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ABSTRACT 


This  final  report  documents  the  work  related  to  tasks  #1  through  14  for  the  project 
entitled: 

“Synthesis  and  Implementation  of  Single  and  Multi-vehicle  Systems  Guidance 
Based  on  Nonlinear  Control  and  Optimization  Techniques". 

with  contract  number  #  W7701-05191 1.  The  work  is  conducted  by  the  team  of  “Control 
and  Information  Systems  (CIS)  Laboratory”  of  Concordia  university  during  2006  and 
2007.  This  report  includes  8  different  chapters  to  fulfill  all  the  tasks  of  the  mentioned 
project  as  follows: 

Chapter  1 :  Tasks  #1,  2,  3  (in  part)  and  5. 

Chapter  2:  Task  #  3  (in  part). 

Chapter  3:  Task  #  4. 

Chapter  4:  Task  #  6. 

Chapter  5:  Task  #  7,  8  and  9. 

Chapter  6:  Task  #  10. 

Chapter  7:  Task#  11. 

Chapter  8:  Task  #  12,  13  and  14. 

It  is  notable  that  task  3  is  accomplished  in  both  chapters  1  and  2. 
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ABSTR.\CT 

This  report  documents  the  work  related  to  tasks  #1,  2,  3  and  #5  for 
the  project  entitled  “Synthesis  and  Implementation  of  Single  -  and 
Multi-vehicle  Systems  Guidance  Based  on  Nonlinear  Control  and 
Optimization  Techniques”.  In  this  report  the  radial  basis  function 
(RBF)  neural  network  control  approach  for  active  flow  control 
extended  to  handle  unmodcllcd  dynamics  and  multiple  equilibria  in 
hybrid  (switching)  system  framework.  Hybrid  RBF  adaptive 
controller  applied  to  delta  wing  vortex-coupled  roll  dynamics  using 
hysteresis  switching  logic.  The  combinatory  control  method  also 
applied  to  the  delta  wing  dynamics  coupled  with  the  SMA  micro 
actuator  dynamics  which  has  been  obtained  form  identification 
process  in  DRDC.  In  this  report  also  the  linear  parameter-varying 
sliding  mode  control  (LPVSMC)  approach  which  has  been 
developed  for  linear  parameter-varying  time-delayed  systems 
(LPVTDS)  has  been  applied  to  delta  wing  model  coupled  with 
SMA  dynamics.  This  approach  combines  sliding  mode  control 
(SMC),  linear  parameter-varying  (LPV)  control  theory,  and  time 
delay  stability  analysis  to  solve  a  LPVTDS  control  problem.  It  is 
anticipated  that  this  method  wilt  lead  to  significant  improvement 
over  existing  SMC  approaches  in  aerospace  applications  with 
parameter  variations  and  coupled  with  new  SMA  actuating 
devices. 
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Nomenclature 


p 

air  density 

/„ 

moment  of  inertia  around  roll  axis 

u  ^ 

free  stream  velocity 

t 

obserc'ation  rime 

q 

dynamic  air  pressure 

T 

time  period  of  decay 

a 

roll  moment  coefficient 

T' 

release  time 

fc 

friction  coefficient 

r 

time  at  step  onset 

wing  element  area 

r 

circulation  or  vortex  strength 

K 

wing  span 

Tc 

cntical  circulauon 

a 

body  axis  inclination 

A 

effective  sweep  back  angle  for  left  vortex 

a 

angle  of  attack 

A, 

empirical  obtained  value  of  sweep  back  angle 

<!> 

roll  angle 

X/. 

non-dimensional  vortex  breakdown  location 

4 

half  apex  angle 

Xv 

static  term  in  Xvb 

Subscripts 

quasi-steady  term 

vb 

v’ortex  breakdown 

s 

static  term 

1 

left  vonex 

u 

unsteady  term 

r 

right  vortex 

h 

time-delayed  vector 

Abbreviations 

SMA 

Shape  Memon,’  Alloy 

SISC) 

Single-Input  Single-Output 

MIMO 

Mulu-Input  Muln-Output 

LMI 

linear  matrix  inequality 

LPV 

linear  paramcter-var\'ing 

LPVSMC 

linear  parameter-v'aning  sliding  mode  control 

LPVTOS 

linear  parameter-varying  time-delayed  system 

SMC 

sliding  mode  ctintrol 

NIRISS 

nonlinear  indicial  response  and  internal  state-space 
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1.1  Section  1:  Introduction,  Preliminaries  and  Motivations 

This  report  addresses  the  following  tasks  related  to  the  projeet  “Synthesis  and 
Implementation  of  Single  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear 
Control  and  Optimization  Techniques”; 

1)  Task  1:  Develop  robust  linear  parameter-varytng  (l.PN')  based  optimal  control  laws  for  an  existing 
time-delaved  flow  control  model 

2)  Task  2:  Extend  artificial  neural  network  (ANN)  control  to  handle  unmodelled  dynamics  and  multiple 
equilibria 

3)  Task  3:  Integrate  outer-loop  flow  control  schemes  with  inner-loop  shape  memory  alloy  actuators 

4)  Task  5:  Build  numencal  models  and  simulations  through  Matlab  validating  the  l.PV  and  ANN  flow 
control  approaches 

In  this  report  the  radial  basis  function  (RBF)  neural  network  control  approach  for  active 
flow  control  introduced  in  references  [1,2, 4, 5]  will  be  extended  to  handle  unmodcllcd 
dynamics  and  multiple  equilibria.  The  theoretical  underpinnings  of  the  method  arc 
developed  in  [6,  7,  8], 

To  deal  with  the  multiple  equilibria  problem,  a  multiple  model  /  multiple  controller  neural 
network  approach  will  be  employed.  The  controller  and  neural  network  will  be  switched  as 
the  system  passes  closer  to  another  equilibrium  point  in  the  model.  Appropriate  switching 
surfaces  will  be  employed  to  achieve  this  objective.  Stability  analysis  of  this  hybrid  system 
will  be  applied  to  ensure  stability  and  the  avoidance  of  chattering  and  limit  cycles  around 
the  switching  surfaces. 

To  handle  the  nonlinear  uncertain  model  of  delta  wing  roll  dynamics  we  will  use  hybrid 
modeling  and  control  notion,  Multiple  RBF  adaptive  controllers  will  be  used  to  control  the 
delta  wing  along  a  prc-spccificd  trajectory.  We  will  also  use  some  logic-based  switching 
rules  in  the  control  process,  and  a  useful  technique  to  suppress  chattering  in  the  switching 
process. 

The  combinatory  robust  adaptive  control  scheme  also  will  be  applied  to  the  delta  wing 
dynamics  coupled  with  the  SMA  micro  actuator  dynamics.  The  SMA  model  has  been 
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obtained  from  an  identification  process  in  DRDC,  For  SMAs  on  the  left  and  right  sides  of 
the  wing  two  different  control  loops  will  be  considered. 

This  report  also  presents  the  application  of  linear  parameter-varying  sliding  mode  control 
(LPVSMC)  design  approach  for  a  LPVTDS  developed  in  [9]  to  delta  wing  model 
coupled  with  SMA  dynamics.  The  organization  of  the  report  is  as  follows.  The  controller 
synreport  conditions  arc  formulated  in  terms  of  LMl  that  can  be  solved  via  LMI  solvers. 
This  result  is  then  generalized  to  multiple  time-delayed  LPV  systems.  The  quasi-LPV 
modelling  and  LPVSMC  synreport  procedure  developed  [9]  for  the  delta  wing  roll 
motion  problem  will  be  used  for  LPVSMC  control. 
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1.2  Section  2:  Hybrid  Adaptive  Control  of  Delta  Wing  Vortex-Coupled 
Roll  Dynamics  Subject  to  Delay 

1.2.1  Introduction 

From  a  conceptual  point  of  view  the  most  basic  definition  of  a  hybrid  system  is  to 
immediately  specify  its  behaviour,  that  is,  the  set  of  all  possible  trajectories  of  the 
continuous  and  discrete  variable  associated  with  the  system.  The  “working  definition”  of 
the  hybrid  systems,  called  the  “hybrids  automaton  model”,  will  provide  the  framework  and 
terminology  to  discuss  the  hybrid  systems  [1].  The  focus  on  hybrid  systems  has  increased 
significantly  in  recent  years  due  to  its  capability  of  modelling  wide  ranges  of  engineering 
applications  such  as; 

•  Aircraft  and  air  traffic  control  systems 

•  Manufacturing  systems 

•  Traffic  control  systems 

•  Hierarchical  control  in  process  industry 

•  Electrical  networks  (circuits  with  diodes  and  switches) 

•  Mechanical  systems  with  collision 

•  Embedded  computation  systems 

The  hybrid  framework  is  ideal  for  modelling  nonlinear  physical  systems,  which  include 
phenomena  that  occurs  at  multiple  time  scales.  In  these  types  of  systems,  the  fast 
dynamics  can  be  abstracted  away  and  be  treated  as  discrete  changes  affecting  the  slower 
dynamic  [2]. 

A  hybrid  system  is  a  mixture  of  continuous-time  and  discrete-event  dynamics  [2,  3,4]. 
Therefore,  the  formulation  of  equations  of  the  hybrid  system  motion  consists  of  two  main 
parts: 
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•  Condition  or  event  rules  (for  discrete  event  component) 

•  Evolution  of  continuous  states  in  every  possible  modes 

By  combining  differential  equations  and  finite  state  automata,  it  is  possible  to  describe  a 
hybrid  system.  It  is  important  to  note  that  different  set  of  differential  equations  describe 
the  continuous  dynamics  in  each  hybrid  systems  mode.  It  means  that  dynamic  structure  of 
a  system  changes  over  the  time  in  accordance  with  its  event  rules.  In  more  details,  the 
number  of  state  variables  and  algebraic  variables  changes  when  switching  among 
different  modes  of  operation  occurs.  These  systems  arc  denoted  as  an  cvolvablc 
structure.  Frame  works  such  as  dynamic  structure  discrete  event  system  specification  and 
object  oriented  physical  modelling  (00PM)  are  introduced  for  the  representation  of  the 
systems  with  cvolvablc  structure  [5].  More  recently,  a  hybrid  system  is  developed  to 
describe  hybrid  dynamics  systems  of  variable  structure  with  varying  number  of  state  and 
algebraic  variables  [6]. 

It  is  worthy  to  mention  that  a  continuous  dynamic  in  each  mode  can  be  described  in 
different  form  of  differential  equations  such  as  implicit/cxplicit  Ordinary  Differential 
Equations  (ODE)  and  implicit/cxplicit  Differential  Algebraic  Equations  (DAE)  [7]. 

The  preliminary  hybrid  automaton  of  delta  wing  system  is  shown  in  Fig.  1 ; 
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One  of  the  issues  which  we  will  encounter  in  the  defining  of  the  switching  logic  in  hybrid 
control  design  process  is  chattering",  which  is  not  desirable.  There  are  basically  two  ways 
to  suppress  chattering:  one  is  called  "hysteresis  switching  logic"  and  the  other  "dwell-time 
switching  logic"  [  1  ]. 

1 .2.2  Tracking  Controller  Structure 

In  order  to  further  improve  performance  when  large  parametric  uncertainties  are  present, 
we  now  design  an  adaptive  sliding  tracking  controller,  where  rolling  moment  coefficient 
(Cl)  as  the  main  uncertain  parameter  is  estimated  on-line.  An  important  feature  of  the 
methodology,  however  is  that  it  provides  a  consistent  rule  for  ceasing  adaptation.  The 
sliding  controller  is  designed  as  if  the  parameters  to  be  estimated  were  known  exactly,  i.c. 
as  if  adaptation  were  successfully  and  exactly  complete.  Since  this  is  initially  no  the  case, 
the  system  first  wanders  outside  the  boundary  layer,  and  the  information  thus  generated  is 
used  to  improve  the  parameters  estimates.  Conversely,  the  scheme  recognizes  that  once  in 
the  boundary  layer  no  advantage  is  gained  by  further  adaptation,  since  even  if  the 
parameters  of  concern  were  exactly  known,  no  improvement  in  tracking  performance  could 
be  guaranteed  formally. 

1.2.3  Preliminaries 

We  try  to  develop  an  adaptive  tracking  control  algorithm  for  dynamic  system  presented  in 
Section  2.  For  this  system  n(t)  is  the  control  input,  Cl  is  an  unknown  non-linear  function. 
The  control  objective  is  to  force  the  plant  state  vector,  X  =(.V3,.X4]^  assumed  available  for 

measurement  to  follow  a  specified  desired  trajectory  •  Defining  the  tracking 

error  vector  x(t)  =  X(t)-  X j{i)  the  problem  is  thus  to  design  a  control  law  u(t)  which  ensures 
that  X(i)  ->  oas  t  . 

Reference  model  which  has  been  applied  to  the  controller  is  as  follows: 
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Kt 

-’C,/(0=  Vj(fi,)+  X"'*'  S'”**.') 

M 

1=1 

M 

XjU)  =  A,  k,'  •i\n(k,l) 

r=l 

A,  =  A,  A-,  =/,  /  =  1,2,...,A/ 

Seeking  agility  and  high  manoeuvrability  capabilities  of  delta  wing  in  high  angle  of 
attack  condition,  a  sophisticated  reference  trajectory  has  been  selected  to  show  the 
controller  eapability  to  control  high  performance  delta  wing. 

Using  the  approaches  presented  in  Ref.  [9]  and  [10],  the  proposed  robust  adaptive  tracking 
controller  structure  is  as  follows: 


=  +»,/  (2) 

The  components  of  the  proposed  eontrollers  are  as  follows: 

A  useful  tracking  error  metric  for  both  sliding  and  adaptive  control  subsystems  is  defined 
by 


5(/)  =  x(0  + (3) 

where T  >  0  .  The  equation  s{i)  =  0  defines  a  time-varying  hyper-plane  in  R-  on  which  the 
tracking  error  vector  decays  exponentially  to  zero  [9].  If  the  magnitude  of  i  can  be  shown 
to  be  bounded  by  a  constant  e  (boundary  layer),  the  actual  tracking  errors  can  be  shown 
[9]  to  be  asymptotically  bounded  by: 

/  =  0,l,...,/7-l  (4) 


1.2.4  Tracking  Controller  Subsystems 

Using  the  metric  s{t)  and  the  saturation  function,  the  control  subsystems  defined  as 
follows: 
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«.(/)  =  [0  A]  ^  -Xj 

X 

(5) 

i<pj=-il<AO  +  a,{t)) 

(6) 

u^i  =-k^,sat{s{t)/  £) 

(7) 

^  =  [-If  iwb)’  T,.;,r(0] 

,v 

=  C;  ^  (X)  =  (  ^  W,  (1)  ^s, 

(8) 

(r)  =  s{t)-s  sat{s{t)/ e) 

(9) 

Adaptation  law: 

(10) 

(11) 

and  k^  are  positivre  constants  which  should  be  chosen  for  each  of  the  control  subsystems. 

The  online  Cl  estimation,  implemented  by  gaussian  radial  basis  function  (RBF)  Neural 
Networks  specially  developed  for  our  case  using  left  and  right  vortex  breakdown  positions, 
can  be  represented  mathematically  as: 


;V 

Q(N)  =  vv, 
/=! 


(12) 


Xvbi  ^(ai,b,)cz  R  &  Xvb,.  =(a^,b^)^  R, 
{inputs  to  the  network ) 


(13) 


=a,  +/A,,  ^|={b, -ai)IN,  4^.  =a^+iA,,  A,  ^{b^ -a^)/ N 
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Si,  =exp(-(A'vif7,  -,^,,)/(2cr;)),  =  exp(-(A'v/),  -^^,)/(2o-;)) 


^Si=  Si,  -Sr, 


(15) 


(16) 


where  g,.  andg^.  are  the  nonlinear  functions  implemented  by  node  /  in  two  subcomponents 
of  the  first  hidden  layer  and  Ag,  is  a  linear  combination  of  two  subcomponents  of  the  first 
hidden  layer  in  second  hidden  layer;  represent  the  input  weights  (or  “center”  in 

the  radial  basis  function  literature)  of  node  i ,  and  m;  represents  the  output  weight  for  that 
node  which  is  updated  using  the  adaptation  law. 

Figure  2  shows  conceptual  illustration  of  RBF  NN  used  for  online  estimation  of 
uncertainty,  which  is  used  in  adaptive  control  component  of  tracking  controller.  This  figure 
shows  the  two  hidden  layers  of  the  network,  one  is  combined  of  two  sub-layers  and  the 
second  hidden  layer  is  a  linear  combination  of  these  two  sub-layers. 
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Fig.  2,  Radial  Basis  Function  (RBF)  neural  network  structure  proposed  for  online  estimation  of 

rolling  moment  coefficient. 


1.2.5  Tracking  Controller  Stability 

To  prove  the  stability  issue  of  our  controller  the  following  Lyapunov  function  candidate 
has  been  proposed  [9]: 


^  Irut  k  ^ 


(17) 


where  w,{i)  =  w,(i)-w,.  Also  the  derivative  is  as  follows: 


^'irack  vV, 

f=l 


(18) 


It  can  be  shown  that  with  some  considerations,  v  <  -k^sj ,  for  all  t  >  and  if  and  the 
entire  are  bounded  at  time  t=0,  they  remain  bounded  for  all  t  >  t^,  the  unifomi 
boundedness  of  by  using  Barbalat’s  lemma  [8J  and  this  establishes  convergence  of 
5^  to  zero.  Hence  the  inequality  s{t)  <  e  holds  asymptotically,  and  the  asymptotic  bounds 
on  the  individual  tracking  errors  follow  using  (26). 


1 .2.6  Hybrid  Controller  Structure 

The  conceptual  stmeture  of  hybrid  RBF  adaptive  controller  is  as  follows.  Fig.  3  is  the 
illustration  of  the  structure  of  hybrid  adaptive  tracking  controller. 
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Roll  Dynamics 

Fig.  3.  Illustration  of  the  hybrid  control  structure. 

The  detailed  hybrid  automaton  for  delta  wing  hybrid  control  case  has  been  illustrated  in 
Fig.  4. 


Fig.  4.  Detailed  Hybrid  automaton  of  the  switching  controller 
The  supervisor  in  Fig.  3  is  called  switching  logic.  One  of  the  main  problems  in  the  design 
of  a  switching  logic  is  that  usually  it  is  not  desirable  to  have  “chattering”,  that  is,  very  fast 
switching.  There  are  basically  two  ways  to  suppress  chattering:  one  is  called  “hysteresis 
switching  logic”  and  the  other  is  “dwell-time  switching  logic”.  Fig.  5  shows  the  hysteresis 
switching  logic  which  has  been  applied  to  the  hybrid  control  of  the  delta  wing  system. 
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Fig,  5.  Hysteresis  switching  logic  for  hybrid  control  of  delta  wing 
This  hysteresis  switching  logic  is  a  way  for  smooth  switching  between  different  models  and 
controllers. 

1.2.7  Numerical  Simulation  and  Discussion 

The  initial  conditions  for  all  of  these  simulations  set  as: 
v(f?):.v,(^)  =  A:j(<7)  =  0,.V3(6')  =  10[deg],xJ6')  =  100[deg/sec]  .  The  complexity  of  the  reference 
trajectory  has  been  defined  by  setting  the  related  parameters  to  be  A  =  0.05,  A/  =  6 .  The 
switching  logic  parameters  are  set  as  follows:  =-20  [deg], =20  [deg],/?,  =4[degj. 

The  parameters  for  the  tracking  controller  of  the  first  region,  have  been  chosen  as 
=  1, =  20,  /I,  =  30,  N^-  20  and  =  0. 1 ,  for  the  second  region 

^d2  -  *<^5/2  -  '>^a2  =  *5’  =  20,yV2=  20  and  =  0.1  and  for  the  third  region 

kj^  -\,k^ij  =10,  /Ij  =  10,2V 3=  20  and  £3  =0.1.  The  simulation  results  for  the  proposed 

controller  are  shown  in  Fig.  6  to  Fig.  15.  Figure  6,  shows  the  time  history  of  the  xy(i)  state 

and  its  reference;  tracking  performance  is  acceptable.  Figure  7,  shows  the  time  history  of 
.V4O)  state  and  its  reference.  Figure  8,  shows  the  time  history  of  .r|(/)  and  xnii)  states.  Figure 
9  and  10,  show  error  and  error  derivative  time  histories.  Figure  1 1,  shows  the  control  input 
time  history;  chattering  in  the  control  input  is  due  to  the  switching  task,  it  has  been 
optimized  by  adjusting  h^.  Figure  12,  shows  the  control  input  components  time  history. 

Figure  13  show  the  sliding  time  history;  as  it  is  apparent  variation  is  inside  the  proposed 
boundary  layer  but  there  are  some  big  jumps.  Figure  14,  shows  the  time  history  of  the  left 
vortex  breakdown  location  on  the  wing.  Figure  15,  shows  the  time  history  of  the  right 
vortex  breakdown  location  on  the  wing.  Figures  show  that  the  controller  is  stable  and  the 
tracking  performance  is  good  enough. 
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Fig.  6  time  history  (plant  output  and  the  command  signal) 
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Fig.  7  ^ and  time  history 
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^irack  components  time  history 


Fig.  12  u  control  input  components  time  history 
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Left  Vortex  Breakdown  Position  time  history 


Fig.  14  Xvbi  time  history 
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Right  Vortex  Breakdown  Position  time  history 

5-- - -  - - - 


1.2.8  References 

[  1  ]  Schaft,  A.,  Schumacher  H.,  An  introduction  to  Hybrid  Dynamical  Systems  (Lecture 
notes  in  control  and  information  sciences  25 1 ),  Springer,  London,  GB,  2000. 

[2]  Mosterman,  P.  J.,  Zhao  F.,  and  Biswas,  G.,  “Model  Semantics  and  Simulation  for 
Hybrid  Systems  Operating  in  Sliding  Regimes,”  Proceedings  of  the  AAAI  Fall 
Symposium  97  on  Model-directed  Autonomous  Systems,  Boston,  MA,  1997,  pp.  48- 
55. 

[3J  Hespanha,  J.  P.,  “Lecture  notes  on  hybrid  control  and  switched  systems”, 

http://www.ece.ucsb.edu/~hespanha/ece229/.  University  of  California  at  Santa 
Barbara,  Fall  2005. 

[4]  Johansson,  K.  11.,  Lygeros,  J.,  Sastry.  S.,  “Modelling  of  Hybrid  systems,”  In  11. 
Unbehauen,  Ed.,  Encyclopedia  of  Life  Support  Systems  (EOLSS),  Theme  6.43: 
Control  Systems,  Robotics  and  Automation.  Developed  under  the  auspices  of 
UNESCO,  2004.  Article-level  contribution.  Invited  paper. 

[5J  Barros,  F.  J.,  Zeigler  B.  P.,  and  Fishwick,  P.  A.,  “Multimodel  and  Dynamic 

Structure  Models:  An  Integration  of  DSDE/DEVS  and  OOPM,”  Winter  Simulation 
Conference,  Washington  DC,  1998,  pp.413-419. 

[6]  Urquia,  A.,  Dormido,  S.,  “Object-Oriented  Description  of  Hybrid  Dynamic 


1-19 


Chapter  I :  Hybrid  Control  Scheme  and  LPV  Sliding  Mode  Control  for  Delta  Wing  Roll  Dynamics  Coupled  with  SMA  Micro  Acmator  Dynamics 


Systems  of  Variable  Structure,”  SAGE  J.  Simulation,  79(9),  2003,  485-493. 

[7]  Brenan,  K.,  Campbell,  S.,  Petzold,  L.,  Numerical  Solution  of  Initial  Value 
Problems  in  Differential-Algebraic  Equations,  Notrh-Holland,  Amsterdam,  1989. 

[8]  Slotine,  J.  -J.  E.,  Li,  W.,  Applied  Nonlinear  Control,  Prentice  1  lall,  Englewood 
Cliffs,  New  jersey,  1991. 

[9]  Slotine,  J.  -J.  E.,  Coetsee,  J.  A.,  “Adaptive  Sliding  Controller  Synthesis  for 
Nonlinear  Systems”,  International  Journal  of  Control,  Vol.  43,  No.  6,  1986,  pp 
1631-1651. 

[10]  Sanner,  R.  M.,  Slotine,  J.  -J.  E.,  “Gaussian  Networks  for  Direct  Adaptive  Control”, 
IEEE  Transactions  on  Neural  Networks,  Vol.  3,  No.  6,  Nov.  1992,  pp  837-863. 


1-20 


Chapter  I :  Hybrid  Control  Scheme  and  LPV  Sliding  Mode  Control  for  Delta  Wing  Roll  Dynamics  Coupled  with  SMA  Micro  Actuator  Dynamics 


1.3  Section  3:  Adaptive  Control  of  Delta  Wing  Roll  Dynamics  Coupled 
with  SMA  Micro-Actuator  Dynamics 

In  this  Section  the  combinatory  control  scheme  developed  in  [1]  will  be  applied  to  the 
model  coupled  with  the  new  SMA  model  developed  in  [2],  The  SMA  control  loop  has 
three  main  blocks:  1)  low  pass  filter;  2)  control  law  with  an  integrator;  3)  SMA  model 
(from  identification  process). 

To  integrate  the  SMA  control  block  in  our  delta  wing  vortex-coupled  dynamics,  the 
discrete  time  model  in  equation  (19)  has  been  changed  to  the  continuous  time  model 
presented  in  equation  (20)  using  Euler  method. 

x{k  +  \)  =  Aj,x{k)  +  Bj,u{k) 
y{k)  =  Cj,xik)  +  Dj,ii(k) 


x(t)  =  A^(i)  + 
y(/)  =  Qx(/)  +  D^,n(7) 

Applying  the  Euler  method  for  numerical  integration  the  following  holds 

x(^  +  l)  =  .v(A')  +  .v7;, 

Replacing  .V  ,  the  following  holds 

x(k  +  1)  =  x(k)  +  {A^.x(k)  +  B^.i({k )) 

Replacing  x(A  +  1) ,  the  following  holds 

Aj,  x{k)  +  Bj,  u{k)  =  (/  +  AJJx{k)  +  7;  u{k) 


(20) 


(21) 


(22) 


(2.3) 
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Bd,  =  BX 


(24) 


So  the  matrices  in  continues  time  form  will  be  computed  as  follows 


A  =  (^d,-nT;' 

=  Bj;' 


C  and  D  will  remain  unchanged 


D,  =  D 


(25) 


(26) 


The  Simulink  block  diagram  of  the  discrete  time  model  of  SMA  control  loop  is  shown  in 
Figure  1. 


Fig.  1  Simulink  block  diagram  of  the  discrete  time  model  of  SMA  control  loop  is  shown 


The  Simulink  block  diagram  of  the  contimwus  time  model  of  SMA  control  loop  is  shown  in 
Figure  2. 
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Fig.  2  Simulink  block  diagram  of  the  continuous  lime  model  of  SMA  control  loop 


Figure  3  shows  a  plot  which  is  the  comparison  between  the  original  discrete  time  model  and 
the  continuous  time  model  we  obtained. 


Fig.  3  Comparison  between  the  discrete  and  continuous  time  SMA  control  loop  simulation 


As  Figure  3  shows,  the  obtained  condnuous  dme  model  difterence  from  the  original  discrete 
dme  model  is  in  an  acceptable  range,  and  it  can  be  used  in  the  simulations. 
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1.3.1  Delta  Wing  Dynamics  Coupled  with  SMA  Antagonistic  Actuation  System 

The  delta  wing  vortex  coupled  model  will  be  as  follows; 


i,  (/)  =  -c  X,  (0  -  £j  -Vj  (/ ) + (/) + X4  (/  -  r) 

X3(/)  =  -fjX3(/)  +  X^(0 

m  =  -q{><)Q-l.x,{t)  +  h,u,{t)ll^. 


(27) 


Where 


^  =  (28) 

^.M(0-^w(40)  +  A',.;W0)A-//).v,(/)  +  a(Dx,(0  +  >w(0  (29) 

A'v.r(0  =  A'^.(x(0)  +  A'^.(x(/))A,(/).v,(0-«(0x,(0+V_  (30) 


The  conceptual  flowchart  of  the  delta  unng  vortex-coupled  dynamics  with  the  new  SMA 
model  is  shown  in  Figure  4. 


Perturbation  ysmai 


Perturbation  ysmar 


Fig.  4  Conceptual  flowchart  of  the  delta  wing  vortex-coupled  dynamics  with  the  new'  SMA  model 
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The  SMA  control  loop  contains  the  continuous  time  model  of  the  following  items: 

1-  Low  pass  filter: 


^/p/(0  =  4/-V;p,(0+5/,./»,(0 


Where 


»,(O  =  «/2-0r-3(O 


(32) 


The  numerical  values  of  the  matrices  for  the  continuous  time  model  arc  as  follows: 

-[O.OOOO  lOO.OOOO],  =[0.0000J  (33) 


■  -20.0000  -1 00.0000’ 

,  ,  = 

’l.OOOO' 

01.0000  -  000.0000 _ 

0.0000 

2-  Control  law  with  an  integrator: 


=  Aonr\on,i()  + 

y conf  (0  =  ^conr  ^cont  (0+  ^conl^<cor,liO 


(34) 


Where 


(35) 


The  numerical  values  of  the  matrices  for  the  continuous  time  model  arc  as  follow's: 


0.0000 

108.5991  -117.8455 

27.6270 

-0.0000 

174.8300  226.4447 

<  A..„,  = 

1 .9049 

0.0000 

-177.2327  -223.8374 

-2.4398 

[l.OOOO  1.0000  O.OOOO],  D.,„„  =  [0.1433]  (36) 


3-  SMA  model  from  system  identification  process: 
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+  D,„, „(0 


(37) 


Where 


(38) 


The  saturation  function  has  been  defines  as  follows: 


Tc<,.t(0^6^.ra/(>’.,„„(0)  =  6 

-  6  <  y,o„,{t)  <  6  ->  .va/(  V’  ,„„,(0)  =  )\-„n, 


(0 


l>Vu.r(0  <  -6  ^  sat{y^.„Jf))  =  -6 


(39) 


The  numerical  values  of  the  matrices  for  the  continuous  time  model  arc  as  follows: 


"177.3034  -177.4760’ 

,  ^  = 

"107.8497  " 

224.4827  -224.2960 

■  imu 

-116.6330 

-  0.0 1 95  0.02 1 0],  =  [O.OOOO]  (40) 


1 .3.2  Combinatory  Controller  Structure 

The  robust  adaptive  tracking  controller  structure  which  has  been  developed  in  [IJ  is  as 
follows: 


t/{0  =  ^^Triuk  (f)  +  {t) 

V 

=  -(.kjs(t)  +  a^(,t))-k,i  sal(x(l)/ £)+{'^  h',(0  Ag,  -/.ijl„y{t) 


(41) 


The  control  inputs  for  the  SISO  model  coupled  with  SMA  actuator  dynamics,  arc  as 
follows: 
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u^(t)-u(t) 

(42) 

Uji!)  =  p,f,,u(l) 

(43) 

(44) 

p^f,,  and p,,,,  are  coefficients  for  the  inputs  to  the  left  and  right  SMA  control  loop. 

The  conceptual  illustration  of  the  combinatory  control  structure  coupled  with  the  new  SMA 
antagonistic  actuation  model  id  shown  in  Figure  5. 


Fig.  5  Conceptual  illustration  of  the  combinatory  control  structure  coupled  with  new 

SMA  model. 

1.3.3  Numerical  Simulation  and  Discussion 

The  initial  condition  for  the  simulation  set  as; 

v((9);A:|(6»)  =  JCj(6')  =  0,.v,((9)  =  30[degl,.v,(6')  =  100[deg/sec]  .  The  complexity  of  the  reference 

trajectory  has  been  defined  by  setting  the  related  parameters  to  be  A  =  0.05,  M  -  6 .  The 
parameters  for  the  tracking  controller  have  been  chosen  as 
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=  1, k,i  =  1, =  1 0,  /I  =  25, yv  =  20  and  £  =  0. 1 .  We  chose  t}^T  =  0.\ [see]  and  - 200  for 
the  stabilizing  control  and  also  we  assumed  =  0.05  .  The  simulation  results  for 

the  proposed  controller  arc  shown  in  Figure  6  to  Figure  17.  Figure  6,  shows  the  time 
history  of  the  x,(0  state  and  its  reference;  tracking  performance  is  acceptable.  Figure  7, 
shows  the  time  history  of  .V4(/)  state  and  its  reference.  Figure  8,  shows  the  time  history  of 
.V|(/)  and  x^it)  states.  Figure  9  and  10,  show  error  and  error  derivative  time  histories. 
Figure  11,  shows  the  control  input  time  history.  Figure  12  and  13  show  the  control  input 
components  time  history.  Figure  14  show  the  sliding  time  history;  as  it  is  apparent  variation 
is  inside  the  proposed  boundary  layer  but  there  arc  some  big  jumps.  Figure  15,  shows  the 
time  history  of  the  left  vortex  breakdown  location  on  the  wing.  Figure  16,  shows  the  time 
history  of  the  right  vortex  breakdown  location  on  the  wing.  Figure  17,  shows  the  output 
time  history  of  the  right  and  left  SMA  control  loops.  Figures  show  that  the  controller  is 
stable  and  the  tracking  performance  is  good  enough  with  the  perturbations  that  we  add  to 
system  by  using  the  SMA  micro-actuators  control  for  left  and  right  halves  of  the  wing. 


Roll  angle  time  Nslory 


Fig.  6  (j)and<j)j  time  history  (plant  output  and  the  command  signal) 
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Roll  rate  time  history 


Fig.  7  time  history 


Variation  of  x,  and  Xj 


time  (sec) 

Fig.  8  .r|and.r2  time  history 


error  time  history 


Fig.  9  error  =  (p-  time  history 
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Fig.  10  ^  =  , 


time  history 


control  input  time  history 
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Fig.  1 1  (overall  control  input)  time  history 
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Right  Vortex  Breakdown  Position  time  history 


Fig.  16  Xvb^  time  history 


Left  &  Right  SMA  micro  actuator  outputs 


Fig.  17  Left  and  Right  SMA  control  loop  output  time  history 
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1.4  Section  4:  Application  of  LPVSMC  to  Vortex-Coupled  Delta  Wing 
Roll  Dynamics  Coupled  with  SMA 

1.4.1  Preliminaries 
Refer  to  ref  [1]. 


1 .4.2  Quasi-LPV  Modelling  for  Delta  Wing  Systems  [  1  ] 

In  this  section,  a  quasi-LPV  model  is  built  on  the  aerodynamic  model  of  vortex-coupled 
delta  wing  systems.  At  first,  the  integral  of  sinusoid  in  equation  (45) 

j'  Xu  (t  -  T)d)(T)dT  =  '  J'  sin(  -|-v---^(i)(T)dT 
•’’-T  tana(t)  ^  T 

is  approximated  as  a  constant  in  several  short  intervals.  Then,  the  integral  is  replaced  by 
the  sum  of  three  time-delayed  components.  Next,  a  state  equation  is  constructed.  In 
addition,  the  time-varying  parameters  arc  defined  and  the  LPV  state  equations  of  the  delta 
wing  systems  arc  obtained. 

In  order  to  approximate  the  integral  in  equation  (45),  the  whole  integral 


range,  t  -  T  <  i  <  t ,  is  divided  into  four  equal  intervals,  i.e. 


T 

t  t — 
4 


T  T 

t —  t — 
4  2 


T 

3T 

and 

3T 

t — 

t - 

t - t-T 

1 

4 

4 

is  reasonable  to  consider  the  sin( 


.  Since  the  size  of  each  interval  is  very  small  (0.04scc),  it 
Tc(t  -t). 


-)  term  as  a  constant.  Now,  we  can  approximate 


the  whole  integral  as 
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sin( 


Tt(t-T) 

T’ 


-)  +  sin(- 


T 


) 


I _ T 


f  ,  <t(x)dT 

I  ^  6(T)dT 


I'  ^  0(T)dT 


- - - ^ - - - Ij  ^(x)dT 


(46) 


From  the  Leibniz-Newton  formula 

[  x(s)ds  =  x(t)-.\(t -h) 

Jl-h 


(47) 


We  can  obtain  the  approximation  of  the  integral  terms 

f  sin(^^^^)ci)(T)dx^0,354(I)(t)+0.5(IXt-T/4)-0.5O(t-3T/4)-0.354O(t-T)  (48) 

Jl-T  "P 

For  simplification,  we  define  a  new  variable  to  represent  the  sum  of  these  three  time 
delays 

^6  =  [0.5O(t-T/4)-0.5O(t-3T/4)-0.354cb(t-T)] 

After  replacing  the  integral  terms  by  time-delayed  representation,  the  vortex  breakdown 
location  can  be  represented  as 


Xvt„(t)  =  X,+ 


1.65 


x.,.(t)=x„- 


tana(t) 

1.65 


1  3 

0.354O(t)-h-5 

2  J 


0.354C>(t)  +  -!-8 

V  2  ; 


tana(t) 

By  substituting  equations  and  into  equation  (52): 

s  0  ^  I (X  “  X  ) 

The  roll  moment  coefficient  Cl(X^j,i,X^j,i)  can  be  represented  as 


(50) 

(51) 

(52) 
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Cl  -  e„  +  e, 


.65 


tan  a(t) 


(0.708c6(t)  +  8  ) 


(53) 


The  LPV  modelling  approach  focuses  on  linearizing  the  nonlinear  systems  by  hiding  the 
nonlinear  terms  via  defining  them  as  time-varying  parameters.  Before  we  constnict  the 
LPV  model  for  the  roll  motion  of  delta  wing  systems,  the  roll  angle  of  delta  wing  is 
defined  as 

X|=0(t)  (54) 

and  the  roll  angular  velocity  of  delta  wing  as 

x,  =  d>(t)  (55) 

It  is  hard  to  analyze  the  systems  and  to  synthesize  the  controller  for  the  steady  terms 


andXj, ,  since  they  are  the  solutions  of  the  second-order  equation  (56). 


C„  +  BX  -AX;  =r 


(56) 


Therefore,  precise  mathematical  expressions  do  not  exist  for  the  tenris  X^,  and  X^^ . 
However,  (X^,  -X^^)  can  be  written  as  a  LPV  representation 


X„-X„  =  p(x,)x, 


(57) 


because  this  term  is  function  of  roll  angle  x, .  Moreover,  the  angle  of  attack  a(t)  is  a 
function  of  the  roll  angle  x, .  This  term  can  be  written  as  a  LPV  representation  mfx,) 
defined  as  another  time-varying  parameter 


1.65 


=  m(x,) 


(58) 


tana(t) 

The  functions  p(X|(t))  and  m(X|(t))  can  be  obtained  by  curv'e  fitting  to  the  experimental 
results  (see  Fig.  1  and  Fig.  2). 
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15, 


—  experimental  results 


'l>(deg) 


Fig.  1  Curve  fitting  X^,  —  versus  the  roll  angle  (j) 


4.3 

—  experimental  results 
^  ^  curve  fitting  results 


it>(deg) 


1.65T 

Fig.  2  Curve  fitting -  versus  the  roll  angle  (j) 

2-tan(a) 


m(X|)  is  a  time-varying  parameter  that  can  be  defined  as 

m(x ,)  =  3.5603  + 1 .9760x  f 


(59) 
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and  p(X|(t))  is  approximated  as  a  constant 

p  =  22.0825  (60) 

Now,  we  only  require  considering  one  time-varying  parameter  m(X|) .  The  parameter  box 
of  m(X|)  is 

m(x,)e  [3.5603,  4.1049]  (61) 

Then,  the  roll  moment  coefficient  Cl  is  expressed  as 

Cl  =  eo  +e|[p(X|)X|  -i-m(X|)(0.708X|  +5  )]  (62) 


=  eo  -he,  [p(X|)  +  0.708m(X|)]x|  +e,  •m(X|)5 

The  LPV  representation  of  the  dynamic  for  the  roll  motion  of  delta  wing  systems  can  be 
written  as 


where 


0  1 
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X2(t) 
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■  0  ■ 
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1 

T21. 

21  ='^^C|[p  +  0.708m(x,)], 


(63) 


a.,  = — r- 


aht2i=^^c,m(x,). 


‘  h22l 


qSab 


qs„b 


e,m(x,). 


aH32,— ^0-708c,-m(x,), 


(64) 


c  =^c 
^21  ,  ^0 


Fig.  3  shows  the  comparison  between  experimental  results  of  the  delta  wing  systems  and 
the  LPV  model.  Fig.  4  and  Fig.  5  show  the  states  phase  plot  of  the  open-loop  LPV  model 
response  to  the  initial  condition  of 
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x,(0)  =  57.3deg  (65) 

X2(0)  =  0  dcg/sec 


Fig.  3  Open-loop  response  of  the  LPV  model  vs.  experimental  results 
400  — 

roll  angle 


time(s) 
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Fig.  4  States  of  the  open-loop  LP\'  model 


400 


0  20  40  60 

roll  angle(deg) 


Fig.  5  Phase  diagram  of  the  open-loop  LP\'  model 

Note  that  the  quasi-LPV  model  developed  in  this  section  is  already  in  a  regular  form  and 
satisfies  Assumption  1  [1].  Thus  we  can  use  the  LPVSMC  synthesis  procedure  discussed 
in  Section  3  of  [1]  to  calculate  the  controller  for  this  LPV  model  directly.  The  following 
section  will  present  the  LPVSMC  synthesis  procedure  for  the  LPV  delta  wing  system 
model. 


1.4.3  Linear  Parameter- Varying  Sliding  Mode  Controller  (LPVSMC)  Design  for  Delta 
Wing  Systems 

In  the  previous  report  [1]  an  approach  was  presented  for  LPVSMC  of  vortex  systems. 
Ref  [1]  presents  a  linear  parameter-varying  sliding  surface  for  regulation  based  on  the 
regular  form  and  synthesizes  a  sliding  mode  controller  with  global  attractivity.  The  error 
signal  is  defined  by 

e  =  r(t)-C(CT)Z|(t) 

where  r(t)  is  the  reference  input  signal.  The  linear  parameter-varying  sliding  surface  is 
chosen  as 
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s(o)  =  S(o,Z|)  +  Z2  (67) 

where  S(o,z  ,)  is  a  linear  parameter- varying  operator  ofz  , ,  The  LPV  operator  S(o,z  ,) 
is  similar  to  the  functionS(Z|),  but  S(o,Z|)  adds  time-varying  parameters  of  the  plant. 
The  parameter-varying  controller  can  adapt  itself  to  the  variation  of  the  system 
parameters  on-line.  Therefore  the  LPVSMC  approach  can  potentially  obtain  better 
control  performance  and  less  conservative  results  than  standard  SMC  with  a  constant 
parameter  sliding  surface.  The  chosen  switching  function  s  will  have  some  dynamics 
compared  to  a  conventional  switching  function  in  terms  of  the  defined  new  variable  z  ^ 

=F(o)z„  +G(o)r-G{o)C(CT)Z| 


S(o,  z  I )  =  H(o)z  „  -f-  L(o)r  -  L{o)C(o)z , 


The  LPVSMC  control  law  is  given  by 


u(t)  = 


l"cq+»„ 


s'B>0 

s'B<() 


where 


(69) 


(70) 
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In  the  following  section  the  approach  is  extended  for  SMA  actuators. 


1 .4.4  Simulation  Results  for  the  Vortex-Coupled  Delta  Wing  System  with  the  Shape 
Memory  Alloy  (SMA)  Micro- Actuator  [2] 

To  include  the  SMA  dynamics  in  the  delta  wing  roll  dynamics,  SMA  model  is  expressed 
as 


X  =  Ax-i-  BU|(t-Tj 
1  1 

U|  = - U|  +  —  u, 

8,  8, 

1  1 

Uj  = - u,  +  —  u 

8,  ‘  8, 

The  states  are  defined  as 

X|  =(t>(t),x,  =cb(t),  x  ,  =U|  andx4=u. 


(72) 


(73) 


The  LPV  representation  of  the  dynamic  for  the  roll  motion  of  delta  wing  systems  can  be 
written  as 
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where 
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1  f* 

a, I  =^^c,[p  +  0.708m(X|)],a„  = — 

a  hi2i  =  -^e ,m(x  ,),a  ,2,,  =  -^e ,m(x , ),a  „2,  - -^0.708e ,  •  m(x , ), 

£,  =  8,  -0.02,  =0.01 


(75) 


Running  ‘PDLKLMIsForDeltaWing4zw3.m’,  we  can  obtain  the  following  results: 


eigPO  = 

2.6068 

2.6068 

2.6068 

2.6068 
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By  using  the  ‘LyapunovDeltawing.m’,  the  simulation  results  can  be  obtained.  In  the 
program,  we  call  ‘f_LyapunovKrasoskii_dot.m’  to  calculate  the  v  dot.  This  function 
using  the  alternative  extended  Simpson’s  rule  to  approximate  the  integral.  Figures  6  to  12 
show  the  simulation  result  for  the  delta  wing  model  coupled  with  2D  SMA  model. 
Figures  are  showing  acceptable  results  for  the  controller  applied  to  the  system  with  SMA 
inputs. 


.|  2  -  reference  signal  r 

closed-loop  LPV  model  output  y 
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Fig.  6  output  time  history 
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Fig.  7  sliding  surface  time  history 
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Fig.  8  sliding  control  input  time  history 
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Fig.  9  (vdl +vd2+vd3)  time  history 
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1.5  Section  5:  Conclusions  and  Future  Work 

Conclusions 

The  multiple  neural  network  adaptive  control  approach  based  on  a  hybrid  system 
framework  has  been  developed  and  applied  to  the  vortex-coupled  delta  wing  dynamics  in 
the  presence  of  uncertainties,  probable  unmodelled  dynamics  and  state  delay.  We  defined 
three  different  RBF  adaptive  controllers  for  three  different  regions.  The  regions  are  defined 
based  on  the  roll  angle  of  delta  wing.  Then  we  tuned  each  controller  for  each  region  for  the 
best  performance.  To  handle  the  chattering  we  used  hysteresis  switching  logic,  using  this 
technique  we  can  minimize  the  chattering  in  our  closed  loop  system.  The  simulation  results 
show  that  the  proposed  switching  controller  can  be  used  for  tracking  control  of  delta  wing 
on  sophisticated  trajectories. 

The  combinatory  robust  adaptive  control  scheme  has  been  applied  to  the  delta  wing 
dynamics  coupled  with  the  new  SMA  micro  actuator  dynamics.  Two  different  control 
loops  has  been  implemented  for  SMAs  on  the  left  and  right  sides  of  the  wing.  The  outputs 
of  these  control  loops  are  the  perturbations  added  to  the  vortex  breakdown  positions.  The 
simulations  show  acceptable  results  with  the  new  dynamics.  However  we  need  to  tune  the 
controller  for  more  perturbations  on  the  wing. 

The  LPVSMC  approach  is  applied  to  a  LPV  representation  of  vortex-coupled  delta  wing 
system  dynamics.  It  is  also  demonstrated  that  the  LPVSMC  approach  is  less 
conservation  than  fixed  parameter  SMC  and  Gouaisbaut’s  SMC  approach.  Furthermore, 
it  is  shown  from  the  simulation  results  that  the  LPVSMC  performance  for  the  nonlinear 
system  is  comparable  or  even  better  than  for  the  LPV  model.  This  indicates  the  approach 
is  also  effective  for  the  original  nonlinear  system  dynamics.  The  approach  presented  is 
one  of  the  first  applications  of  nonlinear  control  for  vortex-coupled  delta  wing  systems 
that  is  verified  on  the  system  with  SMA  actuator  dynamics. 

Future  Work 

For  hybrid  neural  network  adaptive  control  we  can  define  new  parameters  for  our 
switching  logic.  Switching  event  should  be  defined  better  for  better  performance  ol  the 
controller.  It  is  also  possible  to  use  different  neural  networks  like  wavelets  in  the 
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controller  structure.  The  comprehensive  stability  proof  of  the  switching  control  should  be 
carried  out. 

For  the  control  of  the  delta  wing  coupled  with  the  new  SMA  micro  actuator  dynamics  we 
need  to  come  up  to  the  best  control  structure  for  the  perturbations  on  the  wing  to  get  the 
best  control  results  for  the  delta  wing  complicated  roll  manoeuvres.  The  controller  needs 
more  tuning. 

Since  the  LPVSMC  controller  coefficients  depend  directly  on  parameters  to  be  measured 
or  estimated  online,  it  is  anticipated  that  filtering  methods  to  deal  with  parameter 
measurement  noise  or  rapid  parameter  variations  might  be  necessary  in  the  future. 
Another  future  work  is  to  provide  a  more  rigorous  analysis  of  the  robustness  properties  of 
the  LPVSMC  approach,  since  one  of  the  advantages  of  SMC  is  to  achieve  robustness  and 
disturbance  rejection.  The  robustness  investigation  can  be  divided  into  two  steps:  the 
robustness  of  the  attraction  of  sliding  surface  and  the  robustness  on  the  sliding  surface. 
Finally,  it  is  anticipated  that  this  method  will  lead  to  significant  improvement  over 
existing  SMC  approaches  in  aerospace  and  automotive  applications  with  parameter 
variations.  The  new  method  should  be  applied  to  such  applications  to  get  more 
experience  and  help  guide  the  improvement  of  the  method  in  the  future. 
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Abstract 

This  report  documents  the  work  related  to  task  #3  for  the  project  entitled  "Synthesis  and 
Implementation  of  Single  -  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear 
Control  and  Optimization  Techniques”.  Some  advantages  of  Shape  Memory  Alloy 
(SMA)  actuator  make  it  an  attracting  choice  for  How  control  problems;  however,  it 
involves  some  nonlinearity  like  time  delay.  In  this  report.  Receding  Horizon  Control 
(RHC)  method  is  used  to  investigate  the  effects  of  the  SMA’s  time  delay  on  the 
behaviour  of  system.  A  nonlinear  dynamics  of  the  delta  wing  aircraft  which  augments  the 
“vortex  coupled  break  down  location”  for  the  roll  manoeuvre  is  also  used;  in  this 
nonlinear  dynamics,  vortex  location  on  the  wings  is  controlled  by  SMA  actuators. 
Simulation  results  show  that  the  RHC  can  control  the  vortex  coupled  dynamics  of  delta 
wing  for  small  enough  time  delay  (r<  0.25 ). 
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Nomenclature 


h 

wing  span 

K 

controller  gain 

\Q.R 

Weight  matrices  of  quadratic 

cost  function 

a 

angle  of  attack 

dynamic  pressure 

T 

finite  horizon  time 

I 

moment  of  Inertia 

<t> 

roll  angle 

A\b 

Non-dimensional  vortex 

breakdown  location 

fc 

friction  coefficient 

Xs 

static  term  in  Xvh 

s 

wing  element  area 

Cl 

roll  moment  coefficient 

l‘ 

Uncertainty  coefficient 

d 

State  delay 

T 

SMA  actuator  delay 

Subscripts 

vv  wing 

/  left  wing  vortex 


VB  vortex  breakdown 
r  right  wing  vortex 


Abbreviations 


RlIC: 

SMA: 

MIMO 


Receding  Horizon  Control 
Shape  Memory  Alloy 
Multi-Input  Multi-Output 
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2.1  Introduction 

This  report  addresses  the  following  task  related  to  the  project  “Synthesis  and 
Implementation  of  Single  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear 
Control  and  Optimization  Techniques”: 

Task  3:  Integrate  outer-loop  flow  control  schemes  with  inner-loop  shape  memory 

alloy  actuators. 

Shape  Memory  Alloy  (SMA)  actuators  have  some  prominent  abilities  that  make  them 
an  attracting  choice  for  control  of  flow  control  problems;  more  notable,  they  offer  a  more 
compact,  solid  and  cheaper  actuators.  However,  SMAs  have  some  serious  nonlinearity  such 
as  backlash-like  hysteresis.  The  main  idea  to  study  this  backlash  is  to  consider  it  as  a  delay 
in  the  output  of  SMA;  then,  this  delay  will  be  studied  as  a  bifurcation  parameter  while  we 
will  observe  how  its  quantitative  changes  will  affect  the  qualitative  behaviour  of  system. 
Receding  horizon  controller  (RHC)  due  to  its  unique  capability  to  handle  the  constraints 
and  nonlinearity  is  a  good  candidate  to  control  the  delay  dependent  SMA  actuators.  The 
MIMO  dynamics  of  vortex-coupled  roll  dynamics  of  delta  wing  aircraft  is  used  to  study  the 
effect  of  the  time  delay  in  the  control  loop  and  integrating  the  SMA  with  nonlinear 
dynamics  of  vortex  break  down  delta  wing  aircraft. 

In  this  report,  first  the  nonlinear  dynamics  of  “vortex-coupled  roll  dynamics  of  delta 
win  aircraft”  is  introduced.  After  that  the  theoretical  issues  of  a  robust  quasi-RIIC  is 
presented  and  finally  a  quasi-RHC  controller  is  designed  and  implemented  to  the  “vortex- 
coupled  roll  dynamics  of  delta  wing  aircraft”. 
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2.2  MIMO  Dynamics  of  Vortex  coupled  Delta  Wing  with  SMA 

The  “roll  dynamics  of  delta  wing  aircraft  augmenting  a  vortex  coupled  break-down 
location”  is  a  nonlinear  dynamics  containing  state  delay,  uncertainty,  friction  and  zero 
dynamics  with  a  weak  stability  behaviour  of  the  open  loop  system.  The  roll  dynamics  of 
delta  wing  aircraft  augmenting  vortex  coupled  dynamics  is  described  by  [1]: 

'x^{t)  =  cx,{t) 

i,  (/)  =  -c X,  (0  -  -V,  (/)  +  x^  (/)  -1-  X4  (/  -  d) 

Xj{t)  =  -£jXj{t)  +  X,{l) 

K.-Cl(Xvlix{t)))Q-j\  x^{t)  ^ 

Xj  (0  =  ( -  Xj  (0  +  /?2P. w»2  (/  -  ^))  /  ^'1 

X,  (0  =  i-Xf,  (0  +  /?3  Air»3  -  ^))/ 


To  incorporate  the  dynamics  of  shape  memory  alloy  (SMA)  actuators  the  5'’’  and  6'*' 
equations  are  added  to  the  system  dynamics  [1],  hence  the  main  focus  here  is  the  two  last 
equations;  also,  r  is  the  amount  of  delay  induced  to  the  system  and  will  be  discussed 
completely  in  this  report. 

The  rolling  moment  coefficient  C,  is  a  nonlinear  function  of  vortex  breakdown 
locations  [2];  we  assume  it  is  a  3'^'^  order  polynomial  in  the  following  fomi: 

-K,l)  (2) 

Where,  Xm  and  Xvbr  represent  the  vortex  breakdown  locations  for  the  left  and  right 
vortices,  and  andgj  are  constant  coefficients  obtained  using  3'^'^  order  least 

square  curve  fitting  of  the  experimental  data.  Vortex  breakdown  locations  arc  considered 
as  following  [2]: 

=  +  (3) 

^v..(0  =  A'..(0  +  A',.A„(/).v,(0  (4) 

Where,  A',,  and  A,,,  are  static  components  of  vortex  breakdown  locations  in  the 
overall  vortex  breakdown  locations  [3],  and: 
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^^(/)  =  0.91/tan(a(/))  (5) 

Where,  angle  of  attack  (a  )  is  computed  as  follows: 

a(/)  =  atan(cos(  x,  (/))  •  tan(  ct ))  (6) 

And,  bevel  angle  of  the  wing  (<t)  is  an  experimentally  obtained  value  [3],  Equation 
(1)  describes  the  vortex  break  down  roll  dynamics  of  delta  wing  aircraft.  For  a  delta  wing 
aircraft,  the  following  bounds  are  considered  on  the  bank  angle,  roll  rate  and  control 
input: 

-100°  <<!><  100“ 

<  -  100  dcg/scc  <  <  100  deg/ sec  (7) 

-10<i/  <  10 


Then  the  following  bounds  can  be  considered  on  the  states  and  control  inputs,  these 
are  selected  from  the  simulation  results  of  [2]: 

-0.4<X|  <0.4 
-0.4<.v,  <0.4 
-1.74<X3  <1.74  {Rad.) 

-1.74<x,  <1.74  {Rad.) 

<  - 1  <  X5  <  1  (8) 

-l<x,<l 
-10<n,  <10 
-  1  <  z/,  <1 
-l<n3<l 

Choosing  above  parameters  according  to  [2],  we  have: 

c  =  31.4159 
c,  =  0.1 
/„,  =  0.0305 

/c=0.4 
/7|  =  1 

-  (9) 

Q  =27658.97 

/?,  =  /zj  =  0.5 

Pm  =  Pm  =  008 

^  SMA  —0.1 

c/  =  0.1 
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2.3  Quasi  Receding  Horizon  Control 

The  RHC  method  is  based  on  a  repeatedly  on-line  solution  to  a  “finite  horizon  open 
loop  optimal  control”  problem.  An  optimal  control  problem  is  solved  for  a  period  of  time 
called  the  prediction  horizon.  Then,  the  first  portion  or  the  second  portion  of  the 
computed  optimal  input  may  be  applied  to  the  plant  in  a  period  of  time  called  the 
execution  horizon  until  the  next  sampling  of  the  states  becomes  available  [4], 

Some  benefits  of  the  RHC  make  it  a  good  candidate  to  be  used  for  the  flow  control 
problem  of  aerospace  vehicles  which  are  nonlinear  (like  delay),  constrained  and  uncertain 
dynamics.  For  instance,  by  the  means  of  RHC,  it  is  possible  to  design  a  feedback  control 
law  for  nonlinear  systems.  Excellent  capability  to  handle  the  constraints  is  another  unique 
advantage  of  the  RHC.  The  optimality  of  the  RHC  is  also  a  considerable  benefit. 

Suppose  that  the  nominal  model  is  presented  as  the  following  general  form: 

x  =  f{x{t)Mi))  40)==.v„  (10) 

Where  x(/)e'J{”  is  the  state  of  the  system  and  «(/)£'•)?"'  is  the  input  vector 
satisfying  the  constraints: 

u{t)&U  Vf>0  (11) 

U  is  the  set  of  allowable  inputs.  In  order  to  meet  the  necessary  conditions  for 
stability;  it  is  also  assumed  that: 

Al-  /  is  twice  continuously  differentiable. 

A2-  U  is  compaet  and  convex  and  contains  origin. 

A3-  system  (10)  has  a  unique  solution  for  a  given  initial  condition  [4]. 

Then,  under  these  circumstances  the  RHC  is  the  repeated  solution  of  the  following 
problem  [5]: 

Problem  1 :  Find 

T 

min  [i7(.r(r),M(r)>:/r -h  ("(.xtr))  (12) 

ut)  J 

Subject  to 

x(t)  ^  f{x{t),ii{i))  ■v(0)  =  .v„  (13) 

//?„  <(^/(x(0),n(0))  <///?(,  (14) 
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lb,  <  S(x(t),ii{t))<uh,  (15) 

x{T)eQ.^  (16) 

Where 

Q,  =  {v  e  :  F(x)  < /•}  (17) 


t//  is  an  initial  constraint  on  the  parameters  and  5  is  a  trajectory  constraint  enforced 
over  the  entire  time  interval,  lb  and  ub  arc  the  lower  and  upper  bounds  respectively  on 
the  constraints. 

Constraint  (16)  is  added  to  guarantee  the  stability  of  the  RHC;  in  fact,  by  the  means 
of  the  RHC  the  trajectory  of  system  is  driven  to  a  neighbourhood  of  the  origin  and  after 
that  a  linear  local  feedback  controller  stabilizes  the  system;  hence,  for  the  open  loop 
system,  control  input  is  as  follows; 

jn'(x;x(/),0  .x-eA'nQ) 

u(s)  ^  j 

[Kx{s)  X  eQ, 

Where,  X  is  the  set  of  initial  states  that  there  exists  a  feasible  solution  for  them,  is 


(18) 


the  set  of  states  which  do  not  belong  to  Q,.  and  K  is  the  gain  of  local  feedback  controller. 

Repeating  this  computation  yields  a  closed-loop  feedback  control  law.  For  a  receding 
horizon  sampling  period  S  when  S  belongs  to  (0,  7];  then,  the  closed-loop  system  is 
represented  as: 

-v(r)  =  /(-x(r),w‘(r)) 

j/*(r)  =  z/*(r;.x(/))  r  e  [f,f -i- <>'),  0<S<T 

Where f/*(r;,v(0),  r  €  [/,f -i- 7],  is  the  optimal  control  obtained  from  the  above 
problem  with  the  initial  condition  x(t),  and  t  is  the  start  time  of  the  optimization  process 
and  the  instant  at  which  states  arc  sampled. 

One  suitable  choice  for  cost  function  (12)  is  the  quadratic  cost  functions,  because  of 
their  unique  properties  in  the  optimization  problems  [6].  With  the  following  selections, 
the  RHC  problem  becomes  quadratic  [5]: 
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+  (20) 

And; 

V(x{T))  =  ^x^Px  (21) 

Q.Pe'iV""  and  denote  positive-definite,  symmetric  weighting  matrices,  T 

is  a  finite  prediction  time  and  x(f,x„)  denotes  the  trajectory  of  the  system  (10)  driven  by 

u(t)  starting  from  the  initial  condition  xo.  Since  states  and  control  inputs  implicitly 
depend  on  time,  it  is  required  to  parameterize  the  states  and  control  inputs  of  system  as  a 
function  of  time  to  solve  this  optimization  problem.  The  polynomial  basis  function  has 
been  used  for  parameterization  of  states  and  control  inputs. 

2.3.1  RHC  Design  for  MI  MO  Roll  Dynamics 

One  of  the  most  efficient  tools  for  decreasing  the  computation  effort  in  optimization 
problems  is  the  flat  output.  The  main  idea  is  to  reduce  the  dimension  of  the  optimal 
control  problem  to  a  lower  dimension  that  casing  the  formulation  and  reducing  the 
computational  burden  for  optimization  process.  The  motivations  for  using  Hat  outputs  in 
optimization  problems  arc  discussed  completely  in  the  previous  reports  and  interested 
readers  arc  referred  to  [6]  for  more  detail  information  and  application  of  Hat  outputs  in 
the  trajectory  generation  and  optimization  problems. 

To  design  RHC  for  vortex  coupled  delta  wing  dynamics  of  section  1,  consider  the 
following  flat  outputs: 


Then,  all  states  and  inputs  of  the  system  can  be  recovered  as  a  function  of  above  Hat 
outputs  and  their  derivatives: 
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.v,(/)  =  z,(0 
.VjCf)  =  i,(f)/c 
xjio  =  z,(n 
xA')  =  ^2(0  +  ^J^2(0 
xAO  =  -3(0 
xAf)  =  zAO 

“1  (0  =  /„[(^2(0  +  +  ./<(“  2(0  +  ^'d~2U  ))  +  Q-0] 

M2(0  =  ^^(i3(0+’3(0) 

^2PL 

u3(i)  =  -^iiAn  +  ^A0) 

>l3Pr 


The  matrix  penalties  are  selected  as  following; 


Q  =  I 


R  = 


1  0 

0  0.001 

0  0 


0 

0 

0.001 


8.0787 

0.0540 

0.5265 

-0.2271 

0.0000  -0.0000 

0.0540 

8.5553 

0.4820 

0.2856 

0.0000  -0.0000 

0.5265 

0.4820 

9.6056 

0.2916 

0.0000  -0.0000 

-0.2271 

0.2856 

0.2916 

0.0559 

0.0000  -0.0000 

0.0000 

0.0000 

0.0000 

0.0000 

2.9802  -0.0000 

-0.0000 

-0.0000 

-0.0000 

-0.0000 

-0.0000  2.9802 

(23) 


(24) 


Where,  /  is  the  identity  matrix.  P  is  obtained  from  the  Lyapanov  equation  for  the 
lincrized  of  delta  wing  dynamics,  and  also,  Q  and  R  are  tuned  based  on  a  trial  and  error 
procedure  for  the  best  response  of  system. 

It  is  obvious  that  with  larger  finite  horizon  time  T,  the  solution  of  RHC  will  get  closer 
to  the  optimal  solution;  on  the  other  hand,  high  T  will  increase  the  computational  burden; 
hence,  selection  of  T  is  a  trade  off  between  computation  effort  and  optimality.  For  our 
problem  by  a  trail  and  error  procedure  the  optimal  finite  horizon  time  is  chosen  as  T-I 
Sec. 

Also,  the  execution  time  should  be  small  as  much  as  possible  to  meet  the  necessary 
conditions  for  stability  and  performance;  but,  in  reality,  it  is  limited  by  sampling  lime  and 
the  time  required  for  optimization.  Actually,  it  has  to  be  bigger  than  sampling  lime  and 
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optimization  time.  In  our  case,  the  execution  time  is  chosen  as  sec;  in  fact,  it  is  assumed 
that  the  sampling  time  and  the  required  time  for  optimization  arc  smaller  than  0. 1  sec. 

The  quasi  robust  RUC  is  applied  to  the  actual  vortex  coupled  delta  wing  model  with 
all  its  nonlincaritics  including  delay  and  uncertainties  in  the  lift  coefficient;  to  study  the 
robustness  of  the  controller  to  the  disturbances,  a  noise  with  magnitude  of  10%  of  the 
amount  of  states  is  added  to  the  actual  model.  Piecewise  polynomial  class  functions  have 
been  used  to  estimate  the  flat  outputs  as  a  function  of  time;  the  order  of  polynomials  is 
three  with  second-order  continuity  at  break  points. 

For  implementation  of  the  delay  term  in  both  states  and  control  inputs,  a  buffer  is 
used  to  restore  the  amount  of  the  delayed  states  and  control  inputs. 

2.3.2  Simulation  Results  for  Different  Time  Delay  r 

The  simulation  is  done  by  Matlab  software  and  the  related  code  is 
"RHC_Vortex_SMAdelay.m”  that  invokes  two  functions  called:  "jfnnj:hseloop.m"  and 
“model J_nlinear_delay.m''.  The  first  function  represents  the  cost  function  to  be 
minimized  by  RHC  and  the  second  function  calculates  the  lift  coefficient  from  equation 
(2);  this  function  needs  a  data  file  called  “COEFjwnlin.mat”  containing  the  coefficients 
of  the  polynomial  which  is  an  approximation  to  lift  coefficient  (2).  Complementary 
comments  within  the  m-filcs  help  user  to  understand  each  part  of  the  code. 

Simulation  results  for  different  time  delays  are  depicted  in  figures  1  through  24;  the 
time  history  of  all  states,  control  inputs  and  cost  function  are  shown  for  an  initial 
condition.  It  can  be  seen  from  these  simulation  results  that  as  r  increases,  the  5'*’  and  6'*' 
state  trajectories  start  to  oscillate  (figures  5  to  12).  Around  r  =  0.3  (figures  13  to  16),  it 
has  a  periodic  behaviour  like  a  limit  cycle  and  as  r  goes  up,  it  will  get  unstable  gradually 
(figures  17  to  20). 

Although  a  bigger  r  makes  the  and  6*’’  states  unstable  (figures  21  to  24),  it  has  no 
effeet  on  the  other  states.  However,  this  can  affect  the  cost  function;  in  that,  for  higher  r  , 
the  cost  function  may  not  decrease  strictly  (figures  19  and  23). 
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Fig.  1.  Time  history  of  states  for  closed-loop  MIMO  system  for  r  -  0 
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Fig. 2.  Time  history  of  inputs  to  SMA  for  r  =  0 
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Fig. 3.  Time  history  of  cost  function  for  r  =  0 
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Fig. 4.  Time  history  of  5'*^  and  6"’  states  for  r  =  0 


Time,  sec 

Fig. 5,  Time  history  of  states  for  closed-loop  MIMO  system  for  r  =  0. 1 


Time,  sec 


Fig. 6.  Time  history  of  inputs  to  SMA  for  r  =  0. 1 
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Fig. 7.  Time  history  of  cost  function  for  r  =  0. 1 
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Fig. 8.  Time  history  of  5‘*’  and  6’'’  states  for  r  -  0. 1 
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Fig. 9.  Time  history  of  states  for  closed-loop  MIMO  system  for  t  =  0.2 
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Time,  sec 

Fig.  1 0.  Time  history  of  inputs  to  SMA  for  z  -  0.2 
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Fig.l  1.  Time  history  of  cost  function  for  r  =  0.2 
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Fig.  12.  Time  history  of  5’’’  and  6'’’  states  for  r  =  0.2 
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Fig.  13.  Time  history  of  states  for  closed-loop  MIMO  system  for  r  =  0.3 
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Fig.  14.  Time  history  of  inputs  to  SMA  for  r  =  0.3 
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Fig.  15.  Time  history  of  cost  function  for  r  =  0.3 
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Fig.  16.  Time  history  of  5‘''  and  6'*’  states  for  r  =  0.3 
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Fig.  1 7.  Time  history  of  states  for  closed-loop  MIMO  system  for  r  =  0.4 
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Fig.  18.  Time  history  of  control  inputs  for  r  =  0.4 
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Fig.  19.  Time  history  of  cost  function  for  r  =  0.4 
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Fig, 20.  Time  history  of  5'’’  and  6'*^  states  for  r  =  0.4 
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Fig,21 .  Time  history  of  states  for  closed-loop  MIMO  system  for  r  =  0.5 
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Fig. 22.  Time  history  of  control  inputs  for  r  =  0.5 
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Fig. 24.  Time  history  of  5'’’  and  6*'’  states  for  z  =  0.5 
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2.4  Conclusions 

In  this  research,  a  quasi  receding  horizon  control  (RUC)  is  applied  to  the  MIMO 
dynamics  of  vortex  coupled  delta  wing  aircraft  in  roll  manoeuvre  to  study  the  effect  of 
the  time  delay  in  SMA  control  inputs.  Simulation  results  show  that  for  small  enough  r 
(time  delay  of  SMA),  RHC  can  handle  the  SMA’s  time  delay;  in  fact,  as  r  goes  up,  the 
system  becomes  unstable.  According  to  the  simulation  results,  for  :■<  0.25  (time  delay), 
RHC  can  control  the  system  properly. 
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Abstract 

This  report  documents  the  work  related  to  task  #4  for  the  project  entitled 
“Synthesis  and  Implementation  of  Single  -  and  Multi-vehicle  Systems  Guidance 
Based  on  Nonlinear  Control  and  Optimization  Techniques”.  In  this  report,  Receding 
Horizon  Control  (RHC)  method  is  used  for  the  flow  control  problem  of  a  delta  wing 
aircraft  which  augments  the  “vortex  coupled  break  down  location”  for  the  roll 
manoeuvre.  Because  of  nonlincarities  and  uncertainties  in  the  vortex  coupled 
dynamics,  the  existing  ordinary  RHC  methods  can  not  be  applied  to  this  dynamics; 
therefore,  it  is  required  to  develop  a  robust  RHC  scheme.  Moreover,  there  is  no 
design  procedure  to  tunc  the  parameters  of  the  RHC  controller  systematically  for 
nonlinear  systems  with  uncertainty  and  large  number  of  constraints  on  the  states  and 
inputs.  In  this  research,  based  on  a  proposed  robust  quasi  RHC  scheme,  a  design 
procedure  is  proposed  so  that  all  parameters  of  the  R1 1C  controller  can  be  tuned.  The 
stability  of  the  robust  RHC  controller  is  also  proven.  The  effectiveness  of  the  design 
procedure  is  evaluated  by  applying  the  robust  RHC  to  both  the  4'*’  order  and  the 
MIMO  dynamics  of  the  “vortex  coupled  roll  dynamics  of  delta  wing  aircraft”  with 
delay,  unmodcled  uncertainty  and  disturbances.  Simulation  results  show  that  the 
proposed  robust  RHC  can  stabilize  the  vortex  coupled  dynamics  of  delta  wing  with  a 
remarkable  performance.  In  all  cases,  the  region  of  attraction  of  the  robust  RHC  and 
also  terminal  region  is  obtained  and  discussed. 
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Nomenclature 


A,  B 

system  matrices 

L 

Lipchitz  constant 

b 

wing  span 

K 

controller  gain 

P.Q.R 

Weight  matrices  of  quadratic 
cost  function 

a 

angle  of  attack 

dynamic  pressure 

T 

finite  horizon  time 

/ 

moment  of  Inertia 

<t> 

roll  angle 

X.b 

Non-dimensional  vortex 

breakdown  location 

fc 

friction  coefficient 

Xs 

static  term  in  Xvh 

s 

wing  element  area 

Cl 

roll  moment  coefficient 

P 

Uncertainty  coefficient 

Subscripts 

actual 

actual  system 

r 

right  wing  vortex 

cont 

Controller 

sma 

shape  memory  alloy 

1 

left  wing  vortex 

VB 

vortex  breakdown 

Ipf 

Low  Pass  Filter 

w 

wing 

Abbreviations 


RHC 

SMA 

MIMO 

LQR 

RDDNX'AAVCBDL 


Receding  Horizon  Control 
Shape  Memory  Alloy 
Multi-Input  Multi-Output 
Linear  Quadratic  Regulator 

Roll  Dynamics  of  Delta  Wing  Aircraft  Augmenting  Vortex  Coupled  Break-Down 


Location 
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3.1  Introduction 

This  report  addresses  the  following  task  related  to  the  project  “Synthesis  and  Implementation 
of  Single  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear  Control  and  Optimization 
Techniques”: 

Task  4:  Apply  optimization-based  receding-horizon  control  approaches  to  the  flow 
control  problem 

The  RHC  method  is  based  on  a  repeatedly  on-line  solution  to  a  “finite  horizon  open  loop 
optimal  control”  problem.  An  optimal  control  problem  is  solved  for  a  period  of  time  called  the 
prediction  horizon.  Then,  the  first  portion  or  the  second  portion  of  the  computed  optimal 
input  may  be  applied  to  the  plant  in  a  period  of  time  called  the  execution  horizon  until  the  next 
sampling  of  the  states  becomes  available  [1]. 

The  RHC  has  traditionally  found  successful  applications  in  the  chemical  process  control, 
where  the  dynamics  are  slow.  A  very  high  computation  burden  of  the  RHC  has  prevented  its 
application  to  nonlinear  systems  with  fast  dynamics  like  high  performance  aircrafts.  However, 
with  the  advent  of  high  speed  computers  and  the  development  of  the  new  computational 
methods  in  recent  years,  the  RHC  has  been  successfully  applied  to  the  nonlinear  control 
problems  with  fast  dynamics  [2]. 

The  “roll  dynamics  of  delta  wing  aircraft  augmenting  a  vortex  coupled  break-down  location” 
(RDDWAAVCBDL)  is  a  nonlinear  dynamics  containing  state  delay,  uncertainty,  friction  and 
zero  dynamics  with  a  weak  stability  behaviour  of  the  open  loop  system.  It  is  obvious  that  the 
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ordinary  and  typical  existing  RHCs  could  not  be  applied  to  such  a  problematic  and  complex 
dynamics;  consequently,  some  revisions  should  be  made  to  the  ordinary  RHC  schemes. 

The  main  idea  in  this  report  is  to  prove  the  robust  stability  of  the  Quasi  RHC  [3]  for  nonlinear 
systems  with  unmodeled  dynamics.  Then,  based  on  this  robust  RHC  scheme  a  design 
procedure  will  be  developed  so  that  all  matrix  penalties  of  cost  function  [P,  Q,  and  R)  and 
final  horizon  time  (7)  can  be  tuned  for  the  uncertain  systems. 

The  stability  of  the  quasi  RHC  is  proven  in  [3]  for  nonlinear  systems  without  perturbation  and 
any  kind  of  uncertainty.  The  reason  beyond  the  fact  that  the  quasi  RHC  is  used  in  this  study  is 
that  it  is  easy  to  implement  this  kind  of  RHC  to  nonlinear  systems.  Also,  the  computation 
burden  is  less  in  comparison  with  other  kinds  of  RHC  schemes  like  dual-mode  RHC  [4]; 
because,  in  the  quasi  RHC  it  is  not  required  to  apply  the  terminal  constraint.  This  terminal 
constraint  is  used  to  guarantee  the  stability  of  RHC  in  other  kinds  of  RHC  schemes  and 
instead,  some  offline  computations  and  investigations  are  done.  In  other  words,  the  RHC 
controller  parameters  are  tuned  so  that  the  closed-loop  stability  is  guaranteed.  Moreover,  by 
the  means  of  quasi  RHC  it  is  possible  to  discuss  the  region  of  attraction  and  also  terminal 
region  easily. 

Only  two  papers  addressed  the  robust  stability  of  RHC.  In  the  first  one  [5],  authors  used  a  II ^ 
method  to  prove  the  robust  stability  of  the  RHC  for  systems  with  input  disturbances;  however, 
unmodeled  dynamics  and  state  uncertainty  are  not  discussed  in  that  paper.  Also,  in  [4],  the 
robust  stability  of  a  dual  mode  RHC  controller  for  nonlinear  systems  is  discussed 
theoretically;  however,  the  practical  and  implementation  issues  arc  not  discussed.  It  is 
important  to  mention  that  the  dual  mode  RHC  is  not  used  in  today’s  fast  dynamics 
applications  because  of  its  difficulties  and  instead  quasi  RHC  is  being  used  due  to  its 
prominent  benefits. 

In  the  dual  mode  control,  inside  a  terminal  region  a  local  feedback  control  law  and,  outside 
this  terminal  region  an  RHC  is  applied  to  the  system.  Whereas,  in  the  quasi  RHC,  the  RHC 
control  input  is  applied  to  the  system  inside  and  outside  the  terminal  region. 

Some  benefits  of  the  RHC  make  it  a  good  candidate  to  be  used  for  the  How  control  problem  of 
aerospace  vehicles  which  arc  nonlinear,  fast,  constrained  and  uncertain  dynamics.  For 
instance,  by  the  means  of  RHC,  it  is  possible  to  design  a  feedback  control  law  for  nonlinear 
systems.  Furthermore,  since  the  control  law  is  generated  in  a  repeated  fashion,  it  is  possible  to 
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feedback  the  information  through  the  computing  of  each  optimal  control  law.  This  property 
makes  the  RHC  flexible  for  control  of  systems  with  variable  parameters  and  unknown 
environmental  conditions.  Excellent  capability  to  handle  the  constraints  is  another  unique 
advantage  of  the  RHC.  The  optimality  of  the  RHC  is  also  a  considerable  advantage. 

In  this  report,  firstly  the  flow  control  problem  is  discussed;  it  is  indicated  that  the  RHC 
controller  should  fulfill  some  requirements  to  be  applicable  to  flow  control  problem.  Then, 
quasi  RHC  is  introduced  and  the  benefits  of  quadratic  cost  functions  and  flat  outputs  in 
reducing  the  computation  burden  arc  discussed.  Secondly,  some  lemmas  and  theorems  are 
presented  to  prove  the  robust  stability  of  the  RHC  for  uncertain  systems.  We  will  end  up  with 
a  design  procedure  for  nonlinear  systems  with  unmodelcd  dynamics  so  that  all  controller 
parameters  like  penalties  P,  Q,  R,  md  finite  prediction  horizon  time  {T)  is  tuned.  After  that,  to 
verify  the  design  procedure,  a  2"‘’  order  numerical  example  is  presented.  Thirdly,  this  robust 
RHC  is  applied  to  the  4'*’  order  RDDWAAVCBDL  (C,  and  delay  terms  arc  considered  as  the 

unmodelcd  dynamics  and  nonlinearity  correspondingly).  To  incorporate  the  shape  memory 
alloy  (SMA)  actuators,  the  robust  RHC  is  applied  to  MIMO  of  the  RDDWAAVCBDL,  which 
is  a  6'’’  order  dynamics;  this  controller  is  also  applied  to  the  delta  wing  dynamics  with  new 
SMA  antagonistic  actuation  system  in  Appendix  E.  Some  numerical  simulations  arc  done  to 
discuss  the  effectiveness  and  performance  of  the  proposed  robust  quasi  RHC  scheme. 
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3.2  Receding  horizon  control  of  the  flow  control  problem 

Some  features  of  the  flow  control  problem  have  made  it  a  challenging  problem  among  the 
control  designers.  For  instance,  there  are  plenty  of  actuator  saturations  and  constraints  that 
must  be  handled.  Also,  all  manoeuvres  in  flow  control  problem  arc  very  fast.  Furthermore, 
there  arc  some  uncertainties  in  the  model  driving  designers  to  do  an  uncertainty  analysis. 
Some  nonlinear  behaviours  such  as  state  delay  add  also  to  the  problem.  Hence,  an  RllC  has  to 
be  developed  which  can  overcome  all  these  difficulties;  in  fact,  the  RHC  must: 

1-  Handle  all  saturations  and  constraints. 

2-  Be  fast  enough  to  handle  the  fast  dynamics. 

3-  Be  robust  to  unmodelcd  uncertainty  and  disturbances. 

4-  Handle  nonlinearities  like  delay. 

All  these  aspects  are  discussed  in  this  report;  in  fact,  we  will  design  and  formulate  an  RHC 
scheme  that  fulfills  all  of  the  above  requirements. 

Achieving  the  first  requirement  (handling  the  constraints)  is  quite  simple  because  all 
formulations  of  the  RHC  could  handle  all  kinds  of  constraints  and  saturations.  However,  by 
the  means  of  the  quasi  RHC,  this  can  be  done  easily  because  it  is  not  required  to  check  the 
final  constraint  in  the  quasi  RHC  scheme;  this  relaxes  the  final  constraint  and  also  can  reduce 
the  on-line  computation  burden  which  fulfills  the  second  requirement  as  well.  Also,  by 
benefiting  of  the  flat  outputs,  it  is  possible  to  reduce  the  dimension  of  the  optimization 
problem  and  hence  reduce  the  computation  burden  (Appendix  A). 


3.2.1  Quasi  Receding  Horizon  Control 

Suppose  that  the  nominal  model  is  presented  as  the  following  general  form; 

■V  =  f{x{t)Mt))  •v(O)  =  .v„  ( 1 ) 

Where  x(/)e'Jl'’  is  the  state  of  the  system  and  h(/)  e '.H"'  is  the  input  vector  satisfying  the 
constraints: 

u{t)  £  C  V/  >  0  (2) 


3-7 


Chapter  3:  Robust  Quasi  Receding  Horizon  Control  Approach  for  Vortex  Break  down  Augmented  Roll  Dynamics  ofDelta  Wing  Aircraft 


U  is  the  set  of  allowable  inputs.  In  order  to  meet  the  necessary  conditions  for  stability;  it  is 
also  assumed  that: 

Al-  /  is  twice  continuously  differentiable. 

A2-  V  is  compact  and  convex  and  contains  origin. 

A3-  system  ( 1 )  has  a  unique  solution  for  a  given  initial  condition  [  1  ]. 

Then,  under  these  circumstances  the  RllC  is  the  repeated  solution  of  the  following  problem 

[6]: 

Problem  1:  Find 

T 

min  [<7(x(r),w(r)>/r-hF(.v(r))  (3) 

U,.,  J 

Subject  to 

x(l)  =  f{x{t),u(t))  x(0)  =  .v„  (4) 

//)„  <t^{.v(0),i/(0))<f//)o  (5) 

lb,  <  S{xit),ii{t))<uh,  (6) 


x(T)eQ.,. 


(7) 


Where 


Q,  =  (r  6  'jr  :  V(x)  <  r}  (8) 

If/  is  an  initial  constraint  on  the  parameters  and  S  is  a  trajectory  constraint  enforced  over  the 
entire  time  interval,  lb  and  ub  are  the  lower  and  upper  bounds  respectively  on  the  constraints. 
Constraint  (7)  is  added  to  guarantee  the  stability  of  the  RHC;  in  fact,  by  the  means  of  the  RHC 
the  trajectory  of  system  is  driven  to  a  neighbourhood  of  the  origin  and  after  that  a  linear  local 
feedback  controller  stabilizes  the  system;  hence,  for  the  open  loop  system,  control  input  is  as 
follows: 


|f<*(x;x(f),f)  .V  £  yV  nQj 

.V  £Q,. 


(9) 


Where,  X  is  the  set  of  initial  states  that  there  exists  a  feasible  solution  for  them,  Q‘  is  the  set 


of  states  which  do  not  belong  to  Q,.  and  K  is  the  gain  of  local  feedback  controller. 
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Remark:  in  the  quasi  RHC  the  local  feedback  controller  is  used  in  offline  computations  to 
prove  the  stability  of  the  RHC  scheme  and  design  the  cost  function  penalties;  in  that,  in  the 
online  computations  only  the  RHC  controller  scheme  is  used. 

Repeating  this  computation  yields  a  closed-loop  feedback  control  law.  For  a  receding  horizon 
sampling  period  5  when  S  belongs  to  (0,  7];  then,  the  closed-loop  system  is  represented  as: 
jc(r)  =  /(jf(r),M*(r)) 

«*(r)  =  f/(r;  a(/))  r  e  [f,/ +  <>'),  0<S<T 

Where f/*(r;.v(f)),  r  e  [t,t  +  T],  is  the  optimal  control  obtained  from  the  above  problem  with 
the  initial  condition  x(t),  and  t  is  the  start  time  of  the  optimization  process  and  the  instant  at 
which  states  are  sampled. 


3.2.2  Selection  of  appropriate  cost  function  to  achieve  good  stability  and  performance 
properties 

Solving  the  optimal  Problem  1  repeatedly  leads  to  a  closed-loop  feedback  control,  but 
computationally  solving  an  optimal  control  problem  with  cost  function  (3)  is  typically 
demanding  and  time  consuming.  Consequently,  it  is  desired  to  select  ^(.v(0f,«(/))and  V(T) 
such  that  the  computational  effort  in  the  optimization  is  reduced  and  hence,  the  RHC  can  be 
used  for  fast  flow  dynamics.  One  potential  choice  is  quadratic  form  functions  due  to  their 
unique  and  excellent  properties  in  optimization  problems.  In  appendix  B  the  advantages  of 
quadratic  cost  functions  in  the  optimization  problems  are  discussed  completely. 

3.2.3  Quasi  Receding  Horizon  Control  with  Quadratic  Cost  Function 

With  the  following  selections,  the  RHC  problem  becomes  quadratic  [7]: 

q{x{t)Mt))  =  ^x'^{t)Qx{t)  +  ^iJ{t)Ru{t)  (1 1) 

And; 
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F(.r(7’))  =  iv"PA-  (12) 

Q,P  e  IR"’'”  and  R  e  denote  positive-definite,  symmetric  weighting  matrices,  7  is  a  finite 
prediction  time  and  x(f,x„)  denotes  the  trajectory  of  the  system  (1)  driven  by  ii(t)  starting 
from  the  initial  condition  xo. 


3.2.4  Robust  RHC  Approach  for  Nonlinear  Systems 


To  handle  the  uncertainties  and  disturbances,  it  is  required  to  design  a  robust  RllC  scheme  for 
nonlinear  systems;  this  will  fulfill  the  third  and  forth  requirements  of  section  2.  Suppose  the 
nominal  system  (1)  and  the  corresponding  actual  system: 

^  =  ■v(0)  =  x„  (13) 

And  consider  the  Jacobian  linearization  of  the  system  (1)  at  origin  is  described  by: 

X  =  Ax  +  Bu  a(0)  =  Afl  (14) 


Where 


And 


A  =  ^i0,0) 

ox 


B=^{m 

cu 


And  also,  suppose  that: 


actual 


(A(f ),  fdO)  - /(40.  »(0|  ^ /^||j 


(15) 


(16) 


(17) 


To  prove  the  robust  stability  of  the  RHC  for  uncertain  system  ( 13)  with  unmodclcd  uncertainty 
(17),  we  need  the  following  three  lemmas: 


Lemma  1:  there  exists  r  >0  such  that  12,.  is  an  invariant  region  of  attraction  for  nominal 
system  (1)  and  actual  system  (13)  by  the  feedback  control  input  u=kx. 

Proof:  the  existence  of  such  region  of  attraction  for  nominal  system  without  model  error  is 
proven  in  [3];  also,  it  is  proven  that  if 
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A4-  the  Jacobian  linearization  system  (14)  is  stabilizable 
Then,  the  following  Lyapanov  equation  has  a  unique  solution: 

{A,+cIY  P  +  P{A,+cl)  =  -Q'  (18) 

Where, /I*  =  A  +  BK ,  Q’  -  Q  +  K^RK  andc<-/l,„^^(/l* );  and,  is  the  largest  eigenvalue 
of/1^  .  It  is  assumed  that  the  linear  system  (14)  is  stabilizing  with  control  input  u-kx\  then,  all 
eigenvalues  of  are  negative  and  f<- )  guarantees  that  with  positive  definiteness 
ofQ’  =  Q  +  K^RK ,  there  exists  a  unique  positive  definite  solution  P,  for  Lyapanov  equation 
(18). 

To  prove  that  is  an  invariant  region  of  attraction  for  actual  system  with  local  feedback 
control,  first  of  all  find  a  constant  r,  such  that  kx  eU  for  allx  ;  then,  differentiate  fY.v) 
along  the  trajectory  of  actual  system  (13): 

=  -'"ny,,.,,  -  /I..V) +  -x'  /T.' )Px  +  x'\aJP*PA,)x  (19) 

at 

Suppose  that: 

M  =  f-Ai,x  (20) 

Then: 

Wfucua,  -  4  -  Wfacual  “  /||  +  ||/  "  A  ^  (21) 

Also  suppose  that: 


//^  =5n/;{-(^:.veL)  }  (22) 

ll-i 

Then,  inequality  (21)  becomes: 

iLcuai  -  ^^4-  pM\  (23) 

Using  norm,  inequality  (23)  and  using  the  fact  that  the  nonn  of  a  matrix  is  greater  than  any  of 
its  eigenvalues,  we  will  have: 


X^PifaCual  -  ifaaua  “  ||  <  ||/’||||.vf  (//  +  )  < 


If  we  choose  r  such  that: 
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^max(P) 


Then,  from  (25)  and  (24),  we  will  end  up  with: 


The  same  derivation  can  be  done  for: 

actual  -X^  A^k  )PX 

Putting  (26)  and  (27)  into  ( 1 9)  yields: 

cl{V{x))  ^  djx^Px)^  x^O'x 
dt  dt  -  ^  ^ 


(25) 


(26) 


(27) 


(28) 


Inequality  (28)  is  obtained  along  the  trajectory  of  actual  systems  (13)  with  control  input  u=kx 
in  the  setQ,.  and  under  the  condition  (25).  Since  Q  is  positive  definite,  V{xJ  is  decreasing 


inQ,,;  this  implies  that  any  trajectory  of  actual  system  (13)  which  starts  in  Ci^  and  is 
controlled  by  local  feedback  control  u=kx  remains  inQ^;  consequently,  Q,,  is  an  invariant 
region  of  attraction  for  actual  system  (13)  with  local  feedback  control. 

The  next  lemma  discusses  \hQ  feasibility  of  control  input  for  the  actual  system  with  model 
error.  By  the  feasibility  of  RHC  we  mean  that  there  exists  a  valid  control  input  (belongs  to  U) 
and  Si  finite  horizon  time  (T)  so  that  the  system  (13)  is  driven  to  Q,,  at  time  T. 

According  to  [8],  feasibility  of  control  input  at  time  t-0  for  nominal  system  implies  its 
feasibility  for  all  f  >  0  for  nominal  system  without  model  error.  Hence,  for  actual  system  (13) 
with  model  error  (17),  it  is  enough  to  prove  that  there  exists  a  feasible  control  at  time  t-O.  The 
following  lemma  will  discuss  the  conditions  under  which  the  existence  of  a  feasible  control  at 
t=0  is  guaranteed  for  open  loop  system. 


Lemma  2:  for  the  actual  system  (13)  with  model  error  (17),  there  exists  a  valid  r  >0  such  that 
the  actual  system  can  be  driven  to  Q,.  by  the  open  loop  receding  horizon  optimal  control. 

Proof:  suppose  that  there  exist  a  feasible  solution  to  open  loop  Rll  (t,x(i).u,T)  (starting  at  time 
f,  with  initial  condition  x(t),  and  finite  horizon  time  T  )  for  nominal  system  with  Q,, ;  this 
means: 

x(t  +  TleQ,  =>  .v^(f  +  T)Px{t  +  T)<r  (29) 
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In  the  case  of  actual  system,  it  is  obvious  that  if  the  control  input  u(s:  x(r),  t)  is  applied  to  the 
actual  system  for  s  e  [t,t  +  T]  then  the  final  trajectory  x(t+T)  may  not  enter  Q,.  (figure  1)  due 
to  model  error: 


Fig.  1:  Region  of  attraction  and  terminal  region  for  both  actual  system  and  nominal  system 

In  the  above  figure,  Xr(t+T)  denotes  the  final  state  of  actual  system  which  is  different  from  the 
final  state  of  the  nominal  system  and  may  not  enter  Q,.  because  of  model  error.  This  implies 
that  the  terminal  region  must  be  modified  for  the  nominal  system  so  that  the  temiinal  state 
of  the  actual  system,  x/t+T),  enters  into  Hence,  we  have  to  define  a  new  and  a  set 
Q,.  for  nominal  system  so  that  the  trajectory  of  actual  system  enters  intoQ^ . 

Suppose  that  is  the  boundary  of  and  is  the  boundary  ofH^^^^  ;  then,  to  find  the 
setQ,.  ,  we  have  to  push  the  boundaries  of  according  to  following  inequality: 

<b^-\x^(t  +  T;x(t),t)-x{t  +  T-^x{t),t)\  (30) 

To  find  such  a  so  that  the  inequality  (30)  is  satisfied,  we  can  use  the  following  analysis 
along  with  Bellman-Grownwall  lemma  and  Variation  Calculus,  suppose  that: 
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A5-  both  nominal  and  actual  systems  are  Lipchitz  continuous  with  Lipchitz  constant  L  .  Then, 
the  following  is  a  consequence  of  Belman-Grownwall  lemma  for  nominal  system  (1)  and 
actual  system  (13)  with  model  error  (17): 

haauui  -’^(0, 0  -  x{t  +  T,  .v(0T)|| ^ ^ (exp(Z. r)  -  l )  (3 1 ) 

Where,  ||.x||  <  /  on  set  X  (region  of  attraction  of  actual  system  with  RHC). 

Suppose  the  variation  variables  &  and  SI' ,  then: 

V{x)  =  Px=>V{x)  +  SV (,v)  =  x^ Px  +  PSx  +  Sx^  Px  +  SxPSx  (32) 

Omitting  non-variational  terms  yields: 

SV{x)  =  x^  PSx  +  Sx^  Px  +  SxPSx  (33) 

The  last  term  in  the  above  equation  is  the  product  of  two  variation  variables  and  is  negligible; 
hence,  taking  norm  yields: 

||^r(x)||  =  2pPrSc||  <  2pp||||.v||  (34) 

The  term||r5v[|  can  be  calculated  from  (31)  which  is  a  consequence  of  Bellman-Grownwall 


lemma.  Then,  Q,  is  defined  as  following: 

'niv* 


(35) 


Remark:  if  the  model  error  be  such  that  the  right  hand  side  of  (35)  be  zero  (or  negative)  it 
means  that  for  this  actual  system  the  final  terminal  region  set  is  empty  ( )  and  the 
inequality  (7)  converts  to  the  final  equality .v'(f  +  r;.v(0,0- 0  which  is  difficult  to  be 
satisfied  in  online  optimization. 

Then,  with  theQ,.  ,  it  is  obvious  that  for  the  nominal  system  x"^{t  +  T,x{t),t)sQ.^  implies 
that  xl{t  +  T-,x(t),t)eD.^  and  according  to  lemmal  and  since  fijs  an  invariant  region  of 
attraction  for  actual  system,  the  actual  system  will  be  stabilized  by  the  local  feedback  control 
u=Kx  and  then.v'(/  +  S  +  T',x{t  +  S)J  +  S)eQ^  . 
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To  prove  the  stability  of  the  closed  loop  system,  we  use  the  Lyapanov  analysis.  The  quadratic 
performance  index  with  terminal  penalty  is  chosen  as  Lyapanov  function  candidate.  The 
negative  definiteness  of  this  function  for  closed-loop  system  is  approved  in  the  following 
lemma. 

Lemma  3:  suppose  that; 

A6-  There  exists  a  feasible  solution  of  the  RHC  problem  for  the  actual  system  at  time  l  that 
drives  the  final  state  of  actual  system  at  time  f+7  to  Q^(i.e..v'^,,„„, (/ -f- 7’;A'(f),f)eQ^  ).  Also, 

suppose  that  the  linerized  system  of  nominal  system  (1)  is  stabilizing  in  (A4);  then,  there 
exists  a  £■>  0  such  that  the  following  inequality  holds  for  actual  system; 


dt 


,Te[t,t  +  d] 


(36) 


Proof;  Appendix  C. 

Theorem  1.  Suppose  that  the  assumptions  A1-A6  are  satisfied  and  model  error  is  such  that 
there  exists  a  relation  like  (17)  between  nominal  and  actual  system.  Then,  the  closed-loop 
quasi  receding  horizon  control  of  actual  system  with  model  error  is  asymptotically  stable  for 
sufficient  small  execution  time. 

Proof:  we  use  the  definitions  of  asymptotic  stability  in  the  sense  of  Lyapanov  [9J; 

Definition  of  Asymptotic  Stability:  let  x=0  be  an  equilibrium  point  for  (1).  Suppose  that 
V{x)  ;t)r  — >  tR  is  a  continuously  differentiable  function  such  that; 

I.  V(x)  is  positive  definite 

II.  ||.v||  — >  oo=>K(.r)^  00 

III.  K(.v)<0  Vx^O 

Then,  x=0  is  globally  asymptotically  stable. 

Suppose  that; 
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=  +  (37) 

According  to  (11)  and  (12),  +  7')  is  quadratic  function.  Also,  y(xj  is  positive 

definite  because  Q  and  F  are  positive  definite;  condition  II  is  straightforward  for  quadratic 
functions.  Moreover,  from  lemma  3,  III  is  satisfied  and  this  completes  the  proof 

3.2.5  Design  Procedure: 

According  to  previous  lemmas  and  theorem,  in  this  section  a  design  procedure  is  developed  by 
the  means  of  which  parameters  of  the  robust  RHC  controller  is  tuned  so  that  the  stability  of  the 
closed-loop  system  is  guaranteed.  It  is  assumed  that  the  assumptions  A1-A6  hold. 

Stepl.  Define  the  constraints  and  limitations  on  the  states  and  inputs. 

Step2.  Obtain  the  Jacobian  linearized  form  of  nominal  nonlinear  system  (A,  B),  and  check 
whether  the  linerized  system  is  stabilizable  or  not? 

Step3.  Choose  positive  definite  Q  and  R  for  penalties  of  quadratic  perfonnance  index. 

Step4.  By  the  means  of  one  of  linear  feedback  control  methods  like  LQR  obtain  a  linear 
feedback  gain  K  for  linearized  system 
Step5.  Choose  c  so  that  c<-  )  and  Ai^  =  A  +  BK  . 

Step6.  Solve  Lyapanov  equation  (18)  and  obtain  P. 

Step?.  Find  the  largest  possible  o  such  that:  V.v  e  Gf/  ,  (U  is  the  set  of  admissible 

controls). 

Step8.  Find  the  largest  possible  r  G[0,r,  jsuch  that  inequality  (25)  holds. 

Step9.  Choose  T  {Finite  Horizon  time)  and  find  the  largest  possible  e[0,/-]  according  to 
(35)  or  any  other  methods. 

SteplO.  Check  if  with  the  designed  parameters  (P,  Q,  R,  T),  there  is  a  feasible  open  loop 
receding  horizon  optimal  control  for  nominal  system  or  not? 

(i.e.  Jr^(/ +  7’;x(/),/)eQ^^^^ ).  If  not,  either  reduce  the  boundaries  of  X  (region  of 
attraction  of  RHC)  or  go  to  step  3  and  redefine  Q,  R,  T  and  other  design  parameters. 


3-16 


Chapter  3:  Robust  Quasi  Receding  Horizon  Control  Approaeh  for  Vortex  Break  down  Augmented  Roll  Dynamics  of  Delta  Wing  Aircraft 


3.2.6  Numerical  Example: 

The  following  numerical  example  will  verify  the  results.  Suppose  the  following  actual 
nonlinear  system: 

j.v,(0  =  .V2(/)  +  /U,. 1^2(0 

j.Vj  (/)  -  -r,  (/)  -  2x2  (0  +  ii(0(2x,  (f)  +  1)  +  //  ..r,  (0 
Where  terms  multiplied  by  /r,  and  are  unmodeled  dynamics: 


Stepl.  System  is  under  the  following  limitations: 

J- 1  <  X  <  1 
1  <  rr  <  1 


(39) 


Step2,  The  linearization  of  this  system  around  equilibrium  point  (0,  0)  yields: 

|x,(/)  =  X2(/) 

|x,(/)  =  -x,(f)-2x,(/)  +  n(/) 


Both  eigenvalues  of  linearized  system  are  -1;  then  the  linearized  system  is  stabilizable. 
Step3.  Choose: 


Q  =  R  =  l 

(41) 

Step4.  By  the  means  of  LQR: 

K  = 

[-0.4142  -0.41 42 J 

(42) 

steps. 

c  =  0.95 

(43) 

Step6. 

P  = 

■174.7573  117.8077" 

117.8077  80.8581 

(44) 

Step?.  Under  the  conditions  (39),  and  with  K: 


c,  -  491.2308 

(45) 

Steps.  Choosing//  =  0.3 ,  then: 

r  =  195.1239 

(46) 

Step9.  Choosing  1=0.5,  L=l,  T=2  sec, 

=  3.0792 

(47) 
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SteplO.  In  figure  2  these  terminal  regions  are  shown.  Also,  for  some  initial  conditions  the 
open  loop  optimal  control  of  problem  1  is  applied  to  both  nominal  and  actual  system  and 
trajectories  are  depicted.  The  solid  lines  show  the  trajectories  of  actual  system  and  the  dashed 
lines  show  the  trajectories  of  nominal  system.  As  it  is  shown  all  trajectories  of  nominal  system 
enter  into  Q,  and  all  trajectories  of  actual  system  enter  intoQ  ;  this  implies  that  the  robust 

'i*ew 

RHC  controller  with  design  parameters  can  stabilize  the  close  loop  system. 


.  ill  I 

-1  -0.8  -0.6  -0.4  -0.2  0  0.2  0.4  0.6  0.8  1 

X1 


Fig.2.  Terminal  region  for  aetual  system  and  nominal  system  and  trajectories  of  both 
actual  and  nominal  system  subject  to  open  loop  optimal  control 

3.3  Application  to  Vortex  Coupled  Dynamics 

In  this  section,  the  robust  quasi  receding  horizon  control  (RHC)  discussed  in  the  previous 
section  is  applied  to  the  vortex  coupled  dynamics  of  a  delta  wing  aircraft  in  roll  manoeuvre  as 
a  sophisticated  flow  control  problem.  We  start  with  some  discussions  on  the  vortex  coupled 
dynamics;  it  is  required  to  discuss  how  the  delay  term  and  the  uncertainties  of  the  vortex 
coupled  dynamics  are  treated  and  handled  in  the  robust  RHC  design. 


3.3.1  The  time  delay  terms  of  the  vortex  dynamics 

One  of  the  features  of  the  flow  control  problem  which  is  supposed  to  be  handled  by  the  means 
of  robust  RHC  scheme  is  the  state  delay  term.  To  achieve  this  objective,  the  delay  term  is 
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regarded  as  a  nonlinear  term.  Hence,  since  the  designed  robust  RHC  scheme  can  handle  the 
nonlinearities,  it  can  handle  the  delay  term  simply.  According  to  step  2  of  design  procedure, 
this  delay  term  will  be  linearized  as  we  will  see  later  and  the  difference  between  the  nonlinear 
(delay)  term  and  linear  term  is  counted  in  the  design  procedure  of  robust  RHC  design. 
Actually,  this  is  the  advantage  of  the  designed  robust  RHC  controller  that  any  kind  of 
nonlinearity  can  be  approximated  by  a  linear  term  and  then  the  difference  between  the 
nonlinear  term  and  the  corresponding  linearized  is  counted  in  the  design  procedure  to  reduce 
the  approximation  error. 


3.3.2  Robustness  to  unmodeled  dynamics  uncertainty  and  disturbances 

Another  important  feature  of  flow  control  problem  is  unmodeled  dynamics  uncertainty  and 
disturbances  that  must  be  analyzed  in  the  design  procedure.  The  unmodeled  dynamics  in  the 
vortex  coupled  problem  of  delta  wing  is  lift  coefficient  C,;  this  can  be  done  by  the  step  8  and  9 
of  the  design  procedure.  This  is  discussed  thoroughly  in  sections  3.4.1  and  3.5.1 
correspondingly  for  4'*’  order  and  MIMO  dynamics. 


3.3.3  Reduction  of  computation  burden: 

Since  the  RHC  is  a  solution  of  an  optimal  control  problem  which  is  done  online;  then,  always 
a  delay  is  induced  in  the  on-line  applications  known  as  computation  delay.  This  computation 
delay  can  affect  the  stability  and  performance.  To  reduce  the  computation  delay,  it  is  possible 
to  reduce  the  computation  burden.  In  our  proposed  scheme,  this  is  done  in  two  ways:  firstly, 
by  choosing  the  quasi  RHC  scheme;  since  the  final  constraint  in  this  scheme  is  relaxed,  this 
could  reduce  the  computation  burden.  Secondly,  by  reducing  the  final  finite  horizon  time  (T) 
which  is  done  in  the  step  9  of  design  procedure.  In  step  9,  it  is  possible  to  tune  T  for  better 
performance;  the  smaller  T,  the  less  computation  burdens  (the  less  computational  delay). 
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3.3.4  Application  to  4"'  order  Vortex  coupled  Delta  Wing: 

The  roll  dynamics  of  delta  wing  aircraft  augmenting  vortex  coupled  dynamics  is  described  by 

[10]: 

X,(/)  =  CX2(0 

Xj  (/)  =  -cx,  (0  -  X,  (/)  +  X,  (0  +  X,  (/  -  r) 

<  (48) 

,^4 (0  =  iKO ! K  - Cl{Xvlix{l))) Q -  J\.  x^ (0 

The  rolling  moment  coefficient  C,  is  a  nonlinear  function  of  vortex  breakdown  locations  [1 1  j; 
we  assume  it  is  a  3'^‘^  order  polynomial  in  the  following  form: 

ax«x  j=<i  -x*;)+c.(x«  -x  J)  149) 

Where,  Xw  and  Xv6r  represent  the  vortex  breakdown  locations  for  the  left  and  right  vortices, 
and  Co,e|,C2  andc,  are  constant  coefficients  obtained  using  order  least  square  curve  fitting 
of  the  experimental  data.  Vortex  breakdown  locations  arc  considered  as  following  [1  Ij; 

X.,i{t)-X,{t)  +  X,k^{t)x,{t)  (50) 

X.,ri<)  =  XJt)^X^^k^{t)x,{t)  (51) 

Where,  A',,  and  X are  static  components  of  vortex  breakdown  locations  in  the  overall  vortex 
breakdown  locations  [12],  and: 

A'//)  =  0.91/tan(a(f))  (52) 

Where,  angle  of  attack  ( a  )  is  computed  as  follows: 

a(/)  =  atan(cos(  x,  (f))  •  tan(  cr))  (53) 

And,  bevel  angle  of  the  wing  (cr)  is  an  experimentally  obtained  value  [12].  It  is  obvious  that 
by  considering  (49),  dynamics  equation  (48)  becomes  highly  nonlinear;  it  is  desired  to  design 
a  robust  RHC  for  this  nonlinear  dynamics;  the  robust  RIIC  controller  then  can  behave  like  a 
feedback  controller  for  this  nonlinear  system. 


3.4.1.  Robust  RHC  Design  for  4"'  order; 

Equation  (48)  describes  the  vortex  break  down  roll  dynamics  of  delta  wing  aircraft;  in  this 
dynamics  C,  is  regarded  as  an  unmodeled  dynamics. 
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Stepl.  For  a  delta  wing  aircraft  the  following  bounds  are  considered  on  the  bank  angle,  roll 
rate  and  control  input; 

-100° 

- 100  deg/sec  <  ^  <  100  deg/ sec  (54) 

-10<»  <  10 

Then  the  following  bounds  can  be  considered  on  the  states  and  control  input,  these  are  selected 
from  the  simulation  results  of  [1 1]: 

-0.4<x,  <0.4 
-0.4<X2  <0.4 

-  1.74  <JC3  <1.74  (/?aJ.)  (55) 

-1.74<x,  <1.74  {Rad.) 

-10<M<10 


Step2.  Since  the  origin  is  the  equilibrium  of  system  then  the  linearized  system  is: 

'0  c  0  0  1  To 

-c  -£,0  2  0 

A=  5  = 

0  0  -£j\  0 

.0  0  0  -/J  [//,//„ 


(56) 


It  is  important  to  notice  that  the  delay  term  is  approximated  by  a  non-delay  term;  choosing 
above  parameters  according  to  [1 1],  we  have: 


c  =  31,4159 
-  0.1 

/„,  =  0.0305 
■y;  =  o.4 
h^=\ 

0  =  27658.97 


(57) 


Then: 
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0  31.4159 

-31.4159  -0.1000 
0  0 

0  0 


0  0 
0  2.0000 

-0.1000  1.0000 
0  -0.4000 


(58) 


B  =[  0;  0,0;  32.7761] 


The  eigenvalues  of  linerized  system  are: 

-0.0500  +31.4159/ 
-0.0500  -31.4159/ 
-0.1000 
-0.4000 


All  eigenvalues  have  negative  real  part;  then,  the  linearized  system  is  stabilizable. 
Step3. 

Q  =  R  =  I 

Where  /  is  identity  matrix 
Step4.  By  the  means  of  LQR: 

K  =  [0.8700  -1.0250  -0.9019  -1.0742] 


Step5. 


Max(A,^ )  =  -1 .0214  +  3 1 .43 1 7i  ^ 
c  =  0.9 


Step6.  Solving  Lyapanov  equation  for  selected  parameters  above  yields: 


8.0787 

0.0540 

0.5265 

-0.2271 

0.0540 

8.5553 

0.4820 

0.2856 

0.5265 

0.4820 

9.6056 

0.2916 

-0.2271 

0.2856 

0.2916 

0.0559 

(59) 


(60) 


(61) 


(62) 


(63) 


Step7.  For  new  local  controller  to  satisfy  the  input  bounds  (55),  since  U  is  large  enough  this 
can  not  reduce  the  terminal  region;  in  fact,  in  the  above  region  the  local  feedback  controller 
with  gain  of  step  4  is  admissible;  hence,  with  this  new  region  of  attraction: 

r,  =  35.1806  (64) 
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Step8.  In  this  step,  it  is  required  to  obtain  the  uncertainty  parameter  {/j).  In  the  delta  wing 
model,  uncertainty  consists  of  term  C,  ) ;  simulation  results  show  that: 


Max{C,{x^,))  =  Q.0\ 


(65) 


That  barely  happens  and  typically  is  about  0.001 .  For  region  of  attraction  (55),  we  have: 

HI -4-28 


(66) 


Then: 


„  =  0.0023 


(67) 


However,  for  some  other  perturbations  which  are  negligible  in  comparison  with  this 
unmodeled  uncertainty  and  to  avoid  selecting  a  conservative  bound  on  the  uncertainties,  we 
choose: 


/r  =  0.003 


(68) 


Considering  the  delay  term  as  the  nonlinearity,  is  computed  as  following: 


/r,  =A/«x(^)  =  0.4 
\x\\ 


(69) 


Since  in  our  case  the  nonlinearity  is  weak,  no  considerable  changes  happen  for  r: 

r  =  r,  =  35  (70) 

Step9.  It  is  desired  to  have  a  smaller  final  horizon  time  T\  because  the  smaller  T,  the  less 
computation  burden.  Since  the  optimization  time  is  done  on-line,  a  small  decrease  in  T  can 
reduce  the  computation  burden  dramatically;  then,  set  T=1  sec  and  L=6  and: 

/  =  A/av||v||^  =1.74  (71) 


Hence: 


=  13.7070  (72) 

SteplO.  Figure  3  shows  the  trajectory  of  open  loop  nominal  system  with  the  design  parameters 
above  and  with  some  initial  conditions  in  the  region  of  attraction;  the  final  trajectory  of  system 
reaches: 

x(t  +  T;  x(t),  l)  =  [  -0.0395  0.0009  0.4647  -0.6461  /  (73) 

Thus: 
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,v^(f  +  7’)P.v{f  +  n  =  1.9043 


(74) 


Hence,  the  final  trajectory  with  open  loop  enters  Q,.  that  guarantees  the  fact  that  the  final 

trajectory  of  actual  system  enters  Q,..  This  implies  that  this  control  problem  has  a  feasible 
solution;  consequently,  the  closed-loop  RHC  with  designed  parameters  is  stable  for  actual 
system. 

3.4.2.  Closed-Loop  RHC  Design  for  4th  order  roll  dynamics: 

To  design  RHC  for  vortex  coupled  delta  wing  above,  consider  the  following  Oat  outputs; 

(75) 

[z,  =  .X3 

Then,  all  states  and  inputs  of  system  can  be  recovered  as  a  function  of  above  flat  outputs  and 
their  derivatives: 

x,  =  z,  Ic 

■  Vj  =  -2  (76) 

■'^4  =  ^2  + 

.»l  ='^„.[(22  +^^22)  +  /c(22  + 
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Then,  with  designed  parameters  in  previous  section  (P,  Q,  R  and  T)  which  guarantee  the  robust 
stability  of  uncertain  delta  wing  vortex  coupled  model,  the  robust  quasi  RHC  is  applied  to  the 
actual  delta  wing  dynamics  under  constraints  (55).  Also,  to  study  the  robustness  of  the 
controller  to  the  disturbances,  a  noise  with  magnitude  of  10%  of  the  amount  of  states  is  added 
to  the  actual  model. 

Matlab  software  has  been  used  to  simulate  the  system.  Polynomial  class  functions  have  been 
used  to  estimate  the  flat  outputs;  the  order  of  polynomials  is  three  with  second-order 
continuity  at  break  points.  A  sequential  quadratic  programming  (SQP)  [13]  method  has  been 
used  to  solve  the  optimal  control  problem.  Briefly,  in  SQP  method  an  optimization  method,  in 
which  the  cost  function  is  quadratic,  is  solved  by  the  means  of  any  recursive  method  like 
Steepest  Decent  (SD)  method.  In  each  optimization,  the  previous  optimized  value  of 
optimization  parameters  is  used  as  the  initial  guess.  The  time  history  of  states  is  depicted  in 
figure  4  for  an  initial  condition  [0,  0,  58(dcg),  Oj.  The  execution  time  is  J  -  0. 1 . 


-0.8  '  '  ‘ 

0  1  2  3  4  5  6 

time  (sec) 

Kig.4.  time  history  of  states  for  closed-loop  4'*'  order  system 


And  the  input  profile  is  depicted  in  figure  5; 
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8 


As  it  is  shown,  neither  states  nor  input  exeeed  the  limitations  and  constraints  (55)  and  system 
has  a  good  performance  with  designed  parameters.  The  lift  coefficient  which  is  uncertain 
unmodcled  dynamics  is  depicted  in  figure  6. 


Fig6.  Time  history  of  C/ 
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1.4 


1.2 


Fig.7.  Time  history  of  cost  function 


Time  history  of  cost  function  is  depicted  in  figure  7;  as  it  is  shown  the  cost  function  is 
decreasing  which  guarantee  the  stability  of  the  RHC. 


3.3.5  Application  to  MIMO  Dynamics  of  V'ortex  coupled  Delta  Wing: 


The  dynamics  of  vortex  coupled  dynamics  is  described  by: 

X,  (f )  =  -c X,  (/)  -  £j  X,  {t)  +  {t )  +  .V4  (/  -  r) 

\  (77 

X^{t)  =  -£jX^{t)  +  X^{,t) 

x,{t)  =  u{t)IK-CKXvmt)))Q-J\.  x,{l) 

To  incorporate  the  dynamics  of  shape  memory  alloy  (SMA)  actuators  the  following  tlltcr-like 
equations  are  added  to  the  system  dynamics  [10]; 


A5  (/)  =  (  -  A5  (f )  +  /?2  (0)  /  i'/ 

7^6  (0  =  (-^6  (0  +  (0)  / 


(78) 


Equations  (77)  along  with  (78)  are  called  the  MIMO  dynamics  of  roll  dynamics  of  delta  wing 
aircraft  augmenting  the  vortex  coupled  break  down. 
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3.5.1.  Robust  RHC  Design  for  MIMO  Roll  Dynamics: 

Again  a  robust  RHC  is  designed  for  MIMO  dynamics  (Appendix  D);  like  4"’  order,  C,  is 
considered  as  an  unmodeled  dynamics,  all  parameters  of  dynamics  are  selected  like  4"’  order 
case  as  well. 


3.5.2.  Closed-Loop  RHC  Design  for  MIMO  Roll  Dynamics: 

To  design  RHC  for  vortex  coupled  delta  wing  above,  consider  the  following  flat  outputs: 


z,  =  x, 


Z,  =  .X, 


33  =  .V5 


(79) 


L-4 


= 


Then,  all  states  and  inputs  of  the  system  can  be  recovered  as  a  function  of  above  Hat  outputs 
and  their  derivatives: 


X,  = 

.V,  =  Z|  /c 
=  -2 

.V,  =  z,  +  £,,Z, 


X5  =  -4 

“1  =  7„.[(2:  +  )  +  Jci~2  +  )  +  C;.^] 

«3=^(Z3+Z3) 

f^2PL 


U,  = - 


(80) 


>hPr 


Then,  with  the  designed  parameters  of  appendix  D,  and  the  execution  time,<J  =  0.1,  the  quasi 
robust  RHC  is  applied  to  the  actual  vortex  coupled  delta  wing  model  with  all  its  nonlinearities 
including  delay  and  uncertainties  in  lift  coefficient.  Also,  to  study  the  robustness  of  the 
controller  to  the  disturbances  a  noise  with  magnitude  of  10%  of  the  amount  of  states  is  added 
to  the  actual  model.  Again,  similar  to  4’’’  order,  polynomial  class  functions  have  been  used  to 
estimate  the  Hat  outputs;  the  order  of  polynomials  is  three  with  second-order  continuity  at 
break  points.  The  time  history  of  states  is  depicted  in  figure  8.  Also,  time  history  of  control 
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inputs  arc  depicted  in  figures  9  and  10.  Like  4*''  order,  the  performance  of  MIMO  system  is 
good  and  the  robust  ejuasi  RHC  could  stabilize  the  system  property. 


Fig.8.  Time  history  of  states  for  closed-loop  Ml. MO  system 


Fig9.  Time  history  of  inputs  for  closed-loop  MIMO  system 
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3.4  Conclusion 

In  this  research,  a  robust  quasi  receding  horizon  control  (RHC)  is  proposed  and  the  stability  of 
that  is  proven.  Based  on  this  robust  RHC,  a  design  procedure  is  developed  that  enables 
designers  to  tune  the  RHC  parameters  systematically.  The  proposed  robust  RHC  scheme  is 
applied  to  both  4'*’  order  and  MIMO  dynamics  of  vortex  coupled  delta  wing  aircraft  in  roll 
manoeuvre  with  time  delay,  unmodclcd  uncertainty  and  disturbances.  Excellent  simulation 
results  show  that  the  proposed  robust  RHC  scheme  can  be  used  for  flow  control  problems 
efficiently.  Furthermore,  it  is  shown  that  the  controller  can  handle  all  the  constraints  and 
saturations.  In  addition,  in  the  proposed  controller,  some  of  the  computations  arc  done  off-line 
to  meet  the  stability  requirements  and  it  is  not  required  to  fulfill  the  final  constraint  in  on-line 
computations;  hence,  this  can  reduce  the  computation  burden  and  make  it  possible  to  apply 
this  controller  to  fast  dynamics  of  Row  control  problem. 
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3.5  Appendix  A:  Flat  Outputs 

The  dimension  of  the  optimal  control  problem  is  reduced  to  a  lower  dimension  that  casing  the 
formulation  and  reducing  the  amount  of  required  computations  for  optimization  process  by 
means  of  so-called  flat  outputs.  For  applying  the  RllC  method  to  the  roll  dynamics  and 
solving  the  open  loop  optimal  control  problem  the  Hat  output  advantages  has  been  utilized. 
System  (1)  is  called  a  flat  system  if  there  exist  outputs  z  [7]: 

Z=g(x,,)  (81) 

Such  that  the  states  and  the  control  signal  can  be  recovered  from  z  and  its  derivatives;  that  is, 

{x,ii)  =  (82) 

Equation  (82)  is  only  required  to  hold  locally  [7].  For  a  system  being  flat,  it  is  required  that  all 
system  behaviours  including  states  and  control  inputs  can  be  recovered  from  a  finite  number 
of  flat  outputs  derivatives  and  without  integration  by  the  flat  outputs;  hence,  h  has  to  be  a 
smooth  function.  The  interested  readers  are  referred  to  [7]  for  more  details.  If  the  dimension  of 
z  is  smaller  than  that  of  .r  and  u,  the  optimization  problem  is  mapped  to  a  lower  dimension. 
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3.6  Appendix  B:  Quadratic  Cost  Functions 


Suppose  that  the  quadratic  cost  function  is  selected  as: 

0{x)^C\x  +  ^.x^Gx  (83) 

Since  G  is  the  Hessian  of  0  then,  it  is  desired  that  all  eigenvalues  of  G  be  positive  for  a  unique 
global  minimum;  if  some  eigenvalues  of  G  arc  positive  and  some  are  negative;  then  it  means 
there  arc  some  local  weak  minimum  points  that  may  prevent  to  drive  0  to  its  global  minimum 
point;  in  the  case  of  all  negative  eigenvalues  for  G;  there  is  no  minimum  point  [14], 

From  Taylor  scries  expansion: 

0(x+  aP)  =  d>(.v)  +  aP^iGx-^  C)  +  ^a'P^GP  (84) 


So,  for  optimality  [14]: 


Cx’  =-c 


(85) 


Where,  x  is  the  minimum  point  of  0.  If  u^  =  P  is  the  jth  eigenvector  of  G  (Hessian  matrix  of 
0)  then  Uj  is  an  orthonormal  vector;  hence,  Uj  *  =  1 ;  also: 

Gu  ^  (86) 

Then,  from  (84),  (85)  and  (86),  the  following  holds: 

0(.r+ )  =  <I)(.v) +  (87) 

So,  the  changes  in  0  when  moving  away  from  x  along  the  direction  Uj  depend  on  the  sign 
of  : 

« 

If  is  positive;  then,  0  increases,  it  means  that  .v  is  the  minimum  of  0. 

If  A,  is  negative;  then,  0  decreases. 

If  is  zero;  then,  the  value  of  0  remains  constant. 

Also  as  mentioned  above,  when  all  eigenvalues  of  G  arc  positive  then  x  is  the  unique  global 
minimum  of  0;  hence,  the  quadratic  cost  function  will  be  designed  in  such  a  way  that  Hessian 
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matrix  be  positive  definite;  consequently,  all  of  its  eigenvalues  are  positive;  and  it  is 
concluded  that  quadratic  cost  functions  arc  suitable  functions  in  optimal  problems  and  this 
suggests  quadratic  functions  are  good  candidates  for  cost  functions. 
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3.7  Appendix  C:  Proof  of  Lemma  3 

Lemma  3:  suppose  that 


A6-  There  exists  a  solution  to  the  RHC  problem  for  actual  system  at  time  t  that  drives  the  final 
state  of  actual  system  at  time  t+T  to  (i.c. +  T; A'(/),f)€Q^  ),  and  also  suppose  that 


the  linerized  system  of  nominal  system  (1)  is  stabilizing  in  (A4);  then,  there  exists  a  £:>  0 
such  that  the  following  inequality  holds  for  actual  system; 

cV(r;.r(r),/) 


dl 


-<-c  ,re[f,/  +  (?] 


(88) 


Proof:  suppose  that  for  the  actual  system: 


l  +  T 


'^actual  ixit),t,t  +  T)=  |(||x 

ucluul  (  ^  '  x(/),0||g  +  ||a-„,w(^  +  7';A(f),f)||J,  (89) 

t 

Then,  forA/e  [/,/  +  ^]: 

r+Ai+T  ^ 

+  AOT  +  Af,f  +  Af  +  T)=  j  (||A;,,„„,(5;j:(f  +  A/),f  +  Af)||'  +  A/),f  +  Af)||^)t/v 


(90) 


+  (^  +  Af  +  T;  .\(f  +  Af ),  /  +  A/ 


Hence, 


r+r 


Jlcua! (xit  +  M)J  +  At,t  +  Al  +  T)=  I  (||A;„,„,(5;.v(f  +  A/),f  +  A/)||^  +  ||»(.9;x(f  +  A/),/  +  Af)|l‘’  )ds 

I 

/-*.  Af 

|kcma/(‘^;-’f('  +  A/),/  +  Af)||'  +  ||»(.v;x(/  +  Af),/  +  A/)||‘  )ds 


(91) 


l+i\t  +  T  ^ 

+  I  (|Km«/('^;4'  +  A/),/  + Af)||‘  +|m(.v;.v(/  +  A/),/  + A/)||^)f/.9 


^'actual  (t  +  Af  +  T;  x(t  +  Af ),  /  +  Af ) 


Consider  the  following  control  input  over  the  interval)/,/  +  A/  +  T] : 

[//’(xu)/),/)  se[t,t  +  T] 

u{s)  = 

l/fxfx)  i-G[/ +  r,/ +  A/ +  T] 


(92) 
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Where,  K  is  the  gain  of  loeal  feedback  controller.  Since  according  to  Lemma  2  at  time  T  the 
trajectory  of  actual  system  entersQ^  and  inside  this  set,  according  to  lemma  1,  the  feedback 
controller  stabilizes  the  system  then  the  above  controller  is  valid;  feasible  and  stabilizing  for 
actual  system. 

Integrating  of  (28)  over  the  interval  [l  +  T,t  +  At  +  T]  yields: 

n-M*T  p  ^  H-Al+T  f+r+At 

~dr' 


M*T  ,,  T  J3  \  <+A/+r  nT+M 

I  ""  ""  <  |.r'‘0'.r=>||.r(f  +  7’  +  Af)||-  -||.v(/  +  7)||;<-  j(||.v(.Ol|y  +  |KOiri)f/-v  (93) 


t*T 


t^T 


Using  (91 )  and  (93)  the  following  holds: 
actual  -|-A0,/+ +  A/-|-r)  -1-7’) 


t+A( 


+  Af),/  +  A/)|r  +  \\uis-x(t  +  M),  I  +  A/)||^  )Js 


(94) 


Dividing  by  Af  and  taking  limit: 

+  Ar),t  +  At,t  +  At  +  T)  -  J actual  MO,  t,t  +  T) 


Lim 

A/->0 


Ar 

/+A/ 


\c,ualMM  +  ^0,t  +  AOir  +  ||w(-v;-v(/  +  A/),f  +  AO||'Jf/.S- 


(95) 


-  Lim — - 

A;->0 


At 


ac,ualMM),0 

dt 


^  -ilKcualMMlt 


T-,x{t),t)  ■  ) 


And  this  completes  the  proof. 


(96) 
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3.8  Appendix  D:  Robust  RHC  design  for  MIMO  vortex  coupled  dynamics: 


Stepl.  The  constraints  and  limitations  on  states  and  control  inputs  arc  defined  like  4'*'  order 
and  for  the  5'*’  and  6'^  states  and  controls  we  assume  the  following  limitations: 

-  1  <  .V5  <  1 

-l<.v,<l 

•-10<n,  <10  (97) 

-  1  <  n,  <  1 

- 1  <  r/j  <  1 


Step2.  Since  the  origin  is  the  equilibrium  of  system  then  the  linearized  system  becomes: 


0  c  0  0  0  0 

-c  -  0  2  0  0 

0  0  -£j  \  0  0 

0  0  0  0  0 
0  0  0  0  -1  0 

0  0  0  0  0  -1 


B  = 


0 

0 

0 

/?,  1 1 
0 

0 


0 

0 

0 

r  0 

^2  Aw 

^SMA 

0 


0 

0 

0 

0 

0 

^hP.I,r 

^SMA 


(98) 


Choosing  above  parameters  according  to  [10],  we  have: 

/7,  0.5 

'  Aw  =  Aw  =0.08  (99) 

pS.ifA  =0.1 

And  other  parameters  arc  selected  like  the  4‘''  order  case,  then: 
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A 


0 

-31.4159 

0 

0 

0 

0 


31.4159 

-0.1000 

0 

0 

0 

0 


0 

0 

-0.1000 

0 

0 

0 


0 

2.0000 

1.0000 

-0.4000 

0 

0 


0 

0 

0 

0 

-1.0000 

0 


0 

0 

0 

0 

0 

-1.0000 


0  0  0 

0  0  0 

0  0  0 

32.7761  0  0 

0  0.4000  0 

0  0  -0.4000 


eigenvalues  of  linerized  system  are: 

-0.0500 +  31. 41 59i 
-0.0500-31.41591 
-0.1000 
-0.4000 
- 1 .0000 
- 1 .0000 


All  the  eigenvalues  have  negative  real  part  and  then,  linearized  system  is  stable. 
Step3. 

Q  =  R  =  I 


Where,  /  is  identity  matrix. 
Step4.  By  the  means  of  LQR: 


K  - 


0.8700 

0.0000 

0.0000 


-1.0250 

0.0000 

0.0000 


-0.9019 

-0.0000 

0.0000 


-1.0742 

0.0000 

0.0000 


0.0000 

-0.1926 

0.0000 


-0.0000 

-0.0000 

0.1926 


Step5. 


= -1.0034 


c  -  0.9030 


Step6.  Solving  Lyapanov  Equation  for  seleeted  parameters  above  yields; 


(100) 


(101) 


(102) 


(103) 


(104) 
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P  = 


8.0787 

0.0540 

0.5265 

-0.2271 

0.0000  -0.0000 

0.0540 

8.5553 

0.4820 

0.2856 

0.0000  -0.0000 

0.5265 

0.4820 

9.6056 

0.2916 

0.0000  -0.0000 

-0.2271 

0.2856 

0.2916 

0.0559 

0.0000  -0.0000 

0.0000 

0.0000 

0.0000 

0.0000 

2.9802  -0.0000 

-0.0000 

-0.0000 

-0.0000 

-0.0000 

-0.0000  2.9802 

(105) 


Step?.  For  new  local  controller  to  satisfy  the  input  bounds  (55)  and  (99),  since  U  is  large 
enough  this  can  not  reduce  the  terminal  region;  in  fact,  in  the  above  region  the  local  feedback 
controller  with  gain  of  step  3  is  admissible;  hence,  with  this  new  region  of  attraction: 


r,  =  41.1410 


(106) 


Step8.  In  this  step  it  is  required  to  obtain  the  disturbance  parameter  (// ).  In  the  delta  wing 
model  uncertainty  eonsists  of  termC,(.v,^) ;  simulation  results  show  that 

Mav(C;(.r,J)  =  0.01  (107) 

That  barely  happens  and  typically  is  about  0.001  for  region  of  attraction  (55)  and  (99),  we 


have: 


Then; 


U  <6.28 


Max{C,) 


=  0.0016 


(108) 


(109) 


However  for  some  other  perturbations  which  arc  negligible  in  comparison  with  this 
unmodelcd  uncertainty  and  to  avoid  selecting  a  conservative  bound  on  the  uncertainties,  we 
choose: 

/^  =  0.002  (110) 

Considering  the  delay  term  as  the  nonlinearity,  is  computed  as  following: 


/y,=A/av(15^)  =  0.2771 


Since  in  our  case  the  nonlinearity  is  weak,  then  no  considerable  changes  happen  in  r. 

=41 


(111) 


(112) 


Stcp9.  Again  like  4*'’  order  we  want  a  smaller  final  horizon  time;  then,  T=I\  and  also  L-6  and: 
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/  =  Mj-rf-tL  =  1.74  (113) 

Hence: 

=  26.82  (114) 

SteplO.  Figure  1 1  shows  the  trajectory  of  nominal  system  with  the  design  parameters  above 
and  open  loop  optimal  control;  the  final  trajectory  of  system  reaches; 

x(1  +  T:x(t).t)  =  [-Qm26  0.0010  0.6023  -0.2194  0.2926  -0.2926]  (115) 

Then: 

x^{t  +  T)Px{t  +  r)  =  3,9 1 33  <  (116) 

Consequently,  for  MIMO  system,  like  4'’’  order,  there  is  a  feasible  solution  and  with  the 
designed  parameters  the  closed  loop  MIMO  vortex  break  down  roll  dynamics  of  delta  wing  is 
stabilizable. 


time  (sec) 

Figl  1.  Open  loop  trajectory 
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3.9  Appendix  E:  Delta  Wing  Dynamics  Coupled  with  New  SMA 
Antagonistic  Actuation  System 

The  delta  wing  vortex  coupled  model  will  be  as  follows: 

'x,(0-cx2(/) 

X,i()  =  -CX, (0  -£jX,{t)  +  X, (/)  +  X4 (/  -  T) 

1  (117) 

^3(0  =  -^^^3(0 +  ^4(0 

x,(o=-C(K)c-y;x,(o+/v/,(0/C 


Where 


^  =  [X^,,it),X,,^{t)]  (118) 

X,hi iO^  X^,ix{t))  +  X^, (x(/)) ^,  (/)  X,  (f )  +  r/(0 X,  (/ )  +  (119) 

(120) 

The  discrete-time  dynamics  of  new  SMA  model  developed  in  [15]  and  it  is  converted  to 
continuous-time  in  [16],  The  conceptual  flowchart  of  the  delta  wing  vortex-coupled  dynamics 
with  the  new  SMA  model  is  shown  in  figure  12. 
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Perturbation  y^mai 


Fig.  12  Conceptual  llowchart  of  the  delta  wing  vortex-coupled  dynamics  with  the  new  SMA  model 

The  Simulink  block  diagram  of  the  continuous  time  model  of  SMA  control  loop  is  shown  in 
figure  13.  The  SMA  control  loop  has  three  main  blocks:  1)  low  pass  filter;  2)  control  law  with  an 
integrator;  3)  SMA  model  (from  identification  process). 


Fig.l3.  Simulink  block  diagram  of  the  continuous  time  model  of  SMA  control  loop 


The  SMA  control  loop  contains  the  continuous  time  model  of  the  following  items: 


1-  Low  pass  filter: 
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Where 


W,(0  =  W2-Or-3(0 


(122) 


'Fhe  numerical  values  of  the  matrices  for  the  continuous  trme  model  are  as  follows; 


-20.0000  -100.0000 
01.0000  -000.0000 


’  ^Ipf  ~ 


1 .0000 
0.0000 


,  C,„/ -  [o.oooo  100.0000],D,  -[0.0000]  (12.^) 


2-  Control  law  with  an  integrator: 


^cont  (0  =  ^conl^conl  (0  +  ^conl^^conl  (0 

yc.n,i0  =  Cconr^oJO  +  D,o„,U  ,„Jt) 


(124) 


Where 


(125) 


The  numerical  values  of  the  matrices  for  the  continuous  time  model  are  as  follows: 


0.0000 

108.5991  -117,8455 

27.6270 

^lUftl 

-0.0000 

174.8300  226.4447 

.  = 

1 .9049 

0.0000 

-177,2327  -223.8374 

-2.4398 

=  [l  ,0000  1.0000  O.OOOO],  D„„,  =  [0,1433]  (126) 


3-  SMA  model  from  system  identification  process: 


■y.n,ai0  =  ^sma^sma  (0  + 


(127) 


Where 
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The  saturation  function  has  been  defines  as  follows: 


(128) 


^  6  ^  .3Vum(')  =  6 

-  6  <  y,onAt)  <  6  =  T<™,(0 


(129) 


The  numerical  values  of  the  matrices  for  the  continuous  time  model  are  as  follows: 


'177.3034  -177.4760' 

,  = 

'107.8497  ' 

224.4827  -224.2960_ 

’  xmu 

-116.6330_ 

=  [-0.0195 


0.0210],  D, 


[O.OOOO]  (130) 


A  robust  quasi  RUC  controller  is  designed  for  the  delta  wing  dynamics  with  new  SMA  model; 
the  output  of  this  RHC  controller  is  »/;  and  Usma  is  set  to  be  equal  to  10%  of  »/.  All  design 
parameters  are  the  same  as  4‘^  order  case  and  the  time  delay  for  simulations  is r  =  0.1  See. 
Simulation  results  are  depicted  in  figures  14  through  17;  these  figures  show  that  with  the  new 
SMA  model  the  robust  quasi  RHC  can  successfully  control  the  vortex  coupled  dynamics  of 
delta  wing  aircraft. 


Figl4.  Time  history  of  states  with  new  S.MA  model 
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Abstract 

This  report  documents  the  work  related  to  task  #6  for  the  project  entitled  “Synthesis  and 
Implementation  of  Single  -  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear 
Control  and  Optimization  Techniques”.  The  decentralized  control  problem  of  multi- 
UAVs  (Unmanned  Aerial  Vehicle)  consists  of  three  main  problems:  inner-loop  controller 
design,  task  assignment  and  path  planning.  In  this  report,  optimization  based  approaches 
have  been  used  to  study  these  problems.  First,  a  receding  horizon  controller  (RHC)  is 
designed  to  stabilize  the  inner  loop  of  an  under-actuated  3DOF  helicopter  dynamics. 
Then,  another  receding  horizon  optimization  based  approach  will  be  used  for  path 
planning  of  a  fleet  of  UAVs.  An  optimization  based  task  assignment  is  also  designed 
which  minimizes  an  overall  cost  to  go.  The  uniqueness  of  the  decisions  made  by  all 
UAVs  is  guaranteed  by  aiming  the  fact  that  the  global  minimum  of  the  cost  function  is 
unique  for  all  UAVs.  In  this  report,  it  is  assumed  that  the  path  planner  is  fully 
decentralized  and  there  is  no  communication  during  performing  the  mission.  Also,  it  is 
assumed  that  all  UAVs  know  the  initial  position  of  team  members  and  targets  a  priori. 
This  architecture  will  be  triggered  whenever  the  communication  is  not  available  or  the 
communicated  information  is  not  reliable. 


4-2 


Chapter  4;  Fully  Decentralized  Receding  Horizon  Control  (RHC)  Algorithms  for  Cooperative  Path  Planning  and  Trajectory 
Generation  of  Multi-Vehicle  Systems 


Nomenclature 

hmr  Main  rotor  hub  height  above  c.g 
hn-  Tail  rotor  height  above  c.g 

Ixx  rolling  moment  of  inertia 

lyy  pitching  moment  of  inertia 

yawing  moment  of  inertia 

Kp  hub  torsional  stiffness 

Ih!  stabilizer  location  behind  c.g 

l,r  Tail  rotor  hub  location  behind  c.g 

ni  helicopter  mass 

N  Number  of  UAVs 


P,  Q,  R  Weight  matrices  of  quadratic  cost  flit 
P,  q,  r  Angular  velocities 

frontal  fuselage  drag  area 

S^“'  side  fuselage  drag  area 

5>'  vertical  fuselage  drag  area 

T  finite  horizon  time 

n,  V,  w  Local  velocities 

P  Side  slip  angle 

Euler  angles 


Subscripts 

h  Body  axis  (coordinate)  mr 

c  Computation  tr 

s  Stability  axis  (coordinate)  vf 

fus  Fuselage  r 

hi  Horizontal  tail 


Main  rotor 
Tail  rotor 
Vertical  fin 
Real  time 
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4.1  Introduction 

This  report  addresses  the  following  task  related  to  the  project  “Synthesis  and  Implementation 
of  Single  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear  Control  and  Optimization 
Techniques”: 

Task  6:  To  develop  time-constrained  decentralized  receding-horizon  control  (RHC) 
algorithms  for  cooperative  path  planning  and  trajectory  generation  for  multi-vehicle  systems. 
Develop  the  cost  functions,  select  the  numerical  methods  for  optimization  solution,  and  apply 
to  aerial  vehicle  models  as  available  from  the  open  literature. 

UAVs  offer  numerous  advantages  to  the  military  and  civil  applications.  Most  notable  in  the 
military  arc  the  advantages  of  the  ability  to  perform  missions  classified  as  “dull,  dirty,  or 
dangerous”  [1].  Missions  that  are  classified  as  dull  include  examples  of  an  aircraft  loitering  over 
airspace  for  long  periods  of  time  while  providing  surv’cillancc  or  Jamming  enemy  electronic 
devices.  These  types  of  missions  can  last  for  especially  long  periods  of  time,  such  that  manned 
crews  would  not  be  optimal  to  perform.  The  second  type  of  mission  is  the  dirty  type;  this  type  of 
mission  includes  reconnaissance  in  areas  that  have  been  contaminated  by  nuclear,  biological,  or 
chemical  weapons,  where  the  presence  of  manned  aircraft  would  put  the  crew  in  danger.  The  last 
type  is  the  dangerous  mission,  such  as  high-risk  but  high-value  targets  or  Suppression  of  Enemy 
Air  Defences  (SEAD).  These  wide  applications  of  multi-UAVs  missions  motivate  researchers  to 
work  on  the  cooperative  control  of  multi-UAVs;  however,  this  cooperation  must  be  decentralized 
as  much  as  possible  to  increase  the  autonomy  of  each  UAV  for  performing  the  mission  [2]. 
Reduction  of  communication  and  making  the  decision  autonomously  are  two  main  results  of  the 
decentralized  control. 

Figure  (1)  shows  a  typical  cooperative  control  architecture  for  each  UAV.  Depending  on  the 
path  planning  method  or  control  method  this  architecture  may  change;  for  instance  in  some  RllC 
based  architectures  the  control  action  design  and  path  planning  are  done  simultaneously  as  a 
result  of  minimization  of  a  cost  function.  The  cooperative  control  of  multi-UAVs  consists  of 
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three  main  parts  [3]  as  it  is  shown  in  figure  1 :  inner  loop  controller  design,  task  assignment  unit 
and  path  planning  unit: 


Fig  1.  Block  diagram  of  individual  UAV  in  cooperative  control  problem 

The  arrows  show  the  flow  of  information  in  the  above  diagram.  In  this  diagram,  the 
coordinator  behaves  like  an  autopilot  and  based  on  the  information  it  receives  from  the  UAV 
dynamics  and  mission  manager  (which  can  be  a  ground  station),  sends  some  commands  to  the 
task  assignment  and  path  planner  units.  Based  on  the  information  about  the  current  position  of 
UAV  and  the  position  of  the  targets,  the  Task  Assignment  unit  decides  which  task  should  be 
performed  first  and  inputs  the  position  of  that  task  to  Path  Planner  unit.  Afterward,  the  Path 
Planner  Unit  plans  the  paths  for  each  UAV  with  considering  the  collision  avoidance  with 
neighbour  UAVs.  Based  on  the  path  planned  in  the  Path  Planner  Unit,  the  inner  loop  controller 
inputs  the  control  action  to  the  UAV  dynamics  so  that  the  planned  path  is  followed  by  UAV. 
This  procedure  is  performed  on-line  by  each  UAV  during  performing  the  mission. 

Since  designing  an  inner  loop  controller  requires  a  model  of  UAV,  section  4.2.  deals  with  the 
modeling  of  a  small-scale  rotorcraft  UAV  and  deriving  the  equations  of  motion.  The  dynamics  of 
a  helicopter  is  a  nonlinear  and  under-actuated  dynamics  which  is  subject  to  the  saturations  on  the 
control  inputs  and  states.  To  control  this  complex  dynamics,  an  optimization  based  method 
known  as  receding  horizon  control  (RHC)  or  model  predictive  control  (MPC)  will  be  used  to 
design  a  tracking  controller.  The  RHC  can  easily  handle  the  constraints  on  both  inputs  and  states 
and  it  is  a  promising  method  for  designing  feedback  controller  for  the  nonlinear  systems  [4]. 
However,  the  main  drawback  of  the  RHC  is  the  high  computational  burden  that  makes  it  hard  to 
be  applied  to  the  on-line  fast  applications.  Hence,  the  advantages  of  the  flat  outputs  and  B-Splinc 
functions  arc  used  to  alleviate  the  computational  burden  during  the  optimization  process.  The 
idea  of  flat  outputs  is  discussed  in  Appendix  A. 

Although  the  task  assignment  unit  is  not  in  the  scope  of  this  report,  it  is  discussed  shortly; 
because,  the  task  assignment  unit  is  an  important  element  of  the  cooperative  control  and  it  is 
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needed  for  understanding  the  eooperative  scenarios.  Furthennorc,  the  task  assignment  and  path 
planning  arc  strongly  coupled,  and  optimal  coordination  plans  can  not  be  achieved  if  this 
coupling  is  ignored  [3].  The  task  assignment  unit  will  be  addressed  in  section  2.2.  Based  on  the 
available  information  on  the  initial  position  of  the  UAVs,  targets  and  obstacles,  the  task 
assignment  unit  assigns  one  (or  more)  target  to  each  UAV.  It  is  desired  to  develop  a  task 
assignment  unit  which  is  able  to  allocate  tasks  under  imperfection  communication  and  is  reliable 
in  presence  of  uncertain  information  because  in  the  real  world  there  is  often  a  degree  of 
uncertainty  in  the  information  and  the  perfect  information  and  communication  is  not  available; 
hence,  the  task  assignment  unit  should  be  able  to  perform  the  mission  in  presence  of 
uncertainties.  In  the  task  assignment  unit  designed  in  this  report,  an  overall  cost  to  go  for  all 
UAVs  will  be  minimized  to  allocate  the  targets,  and  UAVs  need  only  the  initial  position  of  all 
UAVs  and  targets. 

As  soon  as  the  tasks  arc  allocated  to  the  UAVs,  the  path  planning  begins;  in  fact,  the  task 
assignment  unit  decides  about  the  destination  of  the  UAVs  and  infonns  the  UAVs  about  the 
destination  of  all  UAVs.  Based  on  the  decision  made  by  the  task  assignment  unit,  the  path 
planning  unit  designs  the  paths  for  all  UAVs;  the  paths  starts  from  the  initial  position  of  the 
UAVs  and  have  to  visit  the  targets  while  avoiding  the  obstacles.  Again  like  the  task  assignment 
unit,  in  the  path  planning  unit  it  is  tried  to  reduce  the  amount  of  information  communicated 
among  the  UAVs.  The  planned  paths  are  fed  to  the  controller  (figure  1)  to  provide  the  control 
signal  such  that  the  UAVs  follow  the  planned  paths.  In  this  report,  a  receding  horizon 
optimization  method  is  used  for  path  planning. 

This  report  consists  of  three  main  chapters,  in  the  first  chapter,  the  small-scale  UAV 
rotorcraft  dynamics  will  be  presented  and  for  more  efficiency  in  the  tracking  problems  it  will  be 
presented  in  the  stability  coordinate  [5].  Then,  by  the  means  of  B-Spline  basis  functions,  an  RlIC 
is  designed  to  control  the  inner  loop  3DOF  under-actuated  dynamics  of  helicopter.  In  the  second 
chapter,  after  reviewing  some  common  scenarios  in  the  recent  eontrol  literatures  a  typical 
scenario  for  cooperative  control  will  be  proposed.  An  optimization  based  task  assignment  unit  is 
also  designed  and  finally  different  methods  in  the  path  planning  of  UAVs  are  reviewed.  In  the 
third  chapter,  a  decentralized  receding  horizon  optimization  based  path  planner  is  proposed  to 
perform  the  proposed  scenario.  In  all  cases  the  simulation  results  show  that  the  decentralized 
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receding  horizon  path  planner  can  generate  the  paths  for  multi-UAVs  appropriately  and  the  path 
planner  fulfills  all  requirements. 


4.2  Inner-loop  control  design 

In  this  section,  an  optimization-based  receding  horizon  controller  is  designed  to  control  the 
local  dynamics  of  the  helicopter.  RHC  because  of  its  flexibility  and  prominent  ability  to  handle 
the  constraints  is  a  good  candidate  to  control  the  aerospace  vehicles  which  are  nonlinear, 
constrained,  and  uncertain  and  may  exhibit  under-actuated  dynamics...  The  optimality  of  the 
RHC  is  also  a  considerable  advantage. 

Firstly,  the  6DOF  helicopter  dynamics,  forces  and  moments  in  body  coordinate  is  introduced 
from  the  open  literature;  then,  to  ease  the  outer  loop  tracking  control,  the  dynamics  is  transferred 
to  the  stability  coordinate.  After  that,  a  formulation  of  quasi-infinite-RHC  is  presented  and  by  the 
means  of  B-Spline  basis  functions  and  advantages  of  flat  outputs,  the  RHC  is  implemented  to  the 
nonlinear  3DOF  dynamics  of  helicopter  for  both  stabilization  and  tracking  cases. 

4.2.1  Review  of  Helicopter  Modelling  Literature 

A  mathematical  model  of  the  helicopter  is  required  to  design  and  simulate  the  control  system. 
A  sophisticated  work  was  done  by  MIT  [6],  where  they  developed  an  analytical  and  low-order 
dynamic  model  of  a  miniature  aerobatic  helicopter  in  body  coordinate.  The  model  includes  states 
for  the  longitudinal  and  lateral  states  including  positions,  velocities,  angular  velocities  and  also 
the  control  inputs  including  main  rotor  and  tail  rotor  forces.  Adding  the  Euler  angles  to  the 
dynamic  equations  makes  it  possible  to  make  a  relation  between  the  local  and  global  coordinate 
parameters  for  position  tracking  problems.  The  dynamic  forces  and  moments  which  act  on  the 
helicopter  are  shown  in  figure  2.  Using  the  Newton’s  second  law  for  6  degree  of  freedom 
(6DOF)  yields  the  following  dynamic  equation  in  body  coordinates  [6]: 
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Fig2.  Helicopter  forces  and  moments 
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Where,  the  set  of  forces  and  moments  acting  on  the  helicopter  arc  organized  by  components: 
Omr  for  the  main  rotor.  Op  for  the  tail  rotor,  Qfus  for  the  fuselage  (includes  fuselage  aerodynamic 
effects),  Ovf  for  the  vertical  fin  and  ()ht  for  the  horizontal  stabilizer;  also,  Qe  is  the  engine  torque 
(positive  clockwise).  These  forces  and  moments  arc  discussed  separately  below. 

4. 2.1. 1  .Main  rotor  forces  and  moments 

The  following  forces  and  moments  act  on  the  helicopter  due  to  the  main  rotor  thrust: 

'  =  T,„r  (2) 

Ln,r  =  +  Tn,rb„,r)b, 
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Where,  Kp  is  the  hub  torsional  stiffness,  hmr  is  the  main  rotor  hub  height  above  c.g.,  <:/,  and 

are  coefficients  related  to  the  main  rotor  flapping  angle  and  the  blade  azimuth  angle;  the 
typical  value  of  these  parameters  for  MIT  mini  helicopter  arc  shown  in  table  1. 


4.2. 1.2  Fuselage  forces  and  moments 

The  fuselage  forces  can  be  approximated  by; 


8 

II 

yjul  +  V;  +  (H’„  + 

V 

=  A 

imr 

=  -0,5pS,''"v„v^ 

^  fus 

=  -0.5p5/"(u., 

(3) 


Where,  F^is  the  overall  velocity  of  helicopter,  p  is  the  air  density,  5/'“,  and  5/“  are 

effective  frontal,  side  and  vertical  drag  areas  of  the  fuselage,  v,,  and  m’,,  are  fuselage  center  of 
pressure  velocities  with  respect  to  air  (i.e.  -  u  -  where  is  the  projection  of  wind 

velocity  vector  on  the  A' body  axis). 


4.2. 1.3  Tail  Rotor  and  V'ertical  Fin  forces  and  moments 

The  contribution  of  tail  rotor  and  vertical  fin  is  the  following  forces  and  moments: 

yrr-T,,. 

^,r  =  -TA 

‘  K  =  TuK  (4) 

=  -n. 

Kf  =  y.A 

Where,  l,r  is  the  tail  rotor  hub  location  behind  c.g.  and  h,r  is  the  tail  rotor  height  above  c.g. 
and  is  the  aerodynamic  force  generated  by  vertical  fin.  Table  1  shows  the  physical  MIT  mini 
helicopter  parameters  [6]: 
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4.2.2  3DOF  UAV  dynamics  in  body  coordinate 

In  order  to  simplify  the  problem  for  designing  the  controller,  the  helicopter  dynamics  will  be 
reduced  to  3DOF  dynamics,  ficnee,  the  helicopter  won’t  have  movement  in  the  vertical  direction 
and  roll  and  pitch  orientations;  this  will  make  the  following  parameters  to  zero: 

M’  =  0 

=  0 

P  =  ^  (5) 

6»  =  0 
^  =  0 

Setting  the  above  parameters  related  to  the  vertical  position,  rolling  and  pitching  orientations 
to  zero  in  (1)  yields: 

-  k 

2  mr  ^ 

+  V’ - '-T^r 


u  =  vr-\- 

V  =  -ur  +  c^yylii^ 


m 


+  v” 


•/, 


(6) 


4 

[j/  =  r 


(r  Y 

hr 


Where: 


c,  =--(0.5)p,4,„_ 


m 


(7) 


c,  =--(0.5)p,,4,„^ 


m 


The  last  equation  in  (6)  comes  from  the  Euler  angle  equations.  For  the  positions  in  the  global 
coordinate  we  have: 


(8) 


4  =  i/cos(^)  -  vsin(^) 

=  u sin( y/)^  v cos( y/ ) 

The  main  drawback  of  this  dynamics  is  that  for  tracking  problems  as  we  feed  the  desired 
trajectory  to  the  system,  from  (8),  we  have  2  equations  and  3  unknowns;  therefore,  we  write  the 
dynamics  in  a  coordinate  called  stability  coordinate. 
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4.2.3  Derivation  of  Flight  Equations  in  Stability  Coordinate 

In  the  stability  coordinate,  the  x-axis  is  set  to  be  in  the  direction  of  the  velocity  of  the 
helicopter  [5],  In  figure  3  the  relative  location  and  orientation  of  the  stability  coordinate  and 
body  coordinate  are  shown  for  3DOF  dynamics. 


>'s 

Fig.  3.  Body  and  stability  coordinate 


To  obtain  the  helicopter  dynamics  in  the  stability  coordinate,  we  write  the  Newton’s  second 
law  in  stability  coordinate: 

X  f', ,  =  =>  -  Sin/3  +  D,,^  .Cos/3  =  my,  ( 1 0) 

Where,  D  is  the  drag  force.  From  the  MIT  report  [6]  for  the  drag  forces  we  have: 
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=  (-0-5)  X  f^‘h  ^|^<l  +^l  ( " ) 

D,,^  -{-0.5)xS„,,  p^^Jul+vl  (12) 

The  relation  between  the  velocities  in  the  stability  coordinate  and  body  coordinate  is: 

ic  =11  Cos  B 

n  (13) 

V,  =u^Sin/3 

Using  (13)  in  (1 1)  and  (12),  we  will  have: 

=C^mu]CosP  (14) 


D  =  C  mu]  SinB 

Where: 

C,={-0.5)*S„^p/m 
Cy  =(-0.5)*S/,„  /7/ot 

Also,  from  the  MIT  report  [6]  for  the  main  rotor  force  along  the  x-axis  we  have: 

T  =-k  T 

mr,  ^  mr\  ^  mr 

Then,  Putting  Equations  ( 1 4),  ( 1 5)  and  ( 1 7)  in  (9)  and  ( 1 0)  yields: 

-  T„yCos/3  +  Cymu]  Cos^  ft  +  CpniftShr  ft  =  mx^ 


(15) 


(16) 


(17) 


(18) 


^mr  ~  CyUuftCosft  Sw ft  +  Dui] Sju ft .Cos ft  =  my^  ( 1 9) 

Now,  we  must  find  the  components  of  the  acceleration  in  and  directions;  since  in  the 
stability  coordinate  the  component  of  velocity  in  >’,  direction  is  zero,  we  have: 

V^uJ^+qJ  (20) 

Where  /^and  y'^are  the  unit  vectors.  Differentiation  of  (20)  yields: 

a  =  ^  =  ujy  +uj^  +  his  (21) 

at 

Putting  the  acceleration  components  into  (18)  and  (19)  from  (21),  we  will  have: 

-  T„yCosft  +  CymilCus^ft  +  Cynu]Sirftft  =  mu^  (22) 
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~  C ^mu^Cos/^  Sin/^  +  C,  mil] Siti p .Cos /3  -  mu^  {y/  +  P)  (23) 
Using  the  torque  equations  and  dividing  (22)  and  (23)  by  m  and  rmt^  correspondingly  yields: 

y(- 

ii,  =  Cy;Cos^P  +  C^u]Sm-p - '^T,„,CosP 

m 

k  TSinP 

P  =  -r  -  CosP  SiiiP  +  n,  Sin p. Cos P  +  — ^ - 

mu^  (24) 


[if/ =  r 


Where,  the  last  equation  in  (24)  comes  from  the  Euler  angle  equations.  For  the  positions  in  the 
global  inertia  coordinate  we  have: 

[x^  =  ii.Cos  {p +  1!/) 

[y^.  =  u.Sin  (/?  +  (//) 

Then  we  can  see  from  (25)  that  if  we  impose  the  desired  trajectory,  although  again  we  have  2 
equations  and  3  unknowns,  at  least  forward  velocity  can  be  computed  explicitly  and  we  have  the 
summation  of  the  side-slip  and  yaw  angles.  In  the  next  section,  this  model  will  be  verified  by 
numerical  simulations. 

1.1. 2.1.  Numerical  Verification  of  the  stability  dynamic  model 

In  the  previous  section,  the  helicopter  3DOF  dynamics  is  derived  in  the  stability  coordinate; 

to  verify  the  new  dynamics,  (24)  and  (25)  is  simulated  for  different  kinds  of  inputs.  Then,  the 
output  of  the  stability  dynamics  is  compared  with  the  output  of  the  body  dynamics  (6)  and  (8). 
Using  Matlab  software,  the  simulation  is  done  and  m-filc  associated  with  this  simulation  is 
"'model _verification.m" .  The  simulation  results  for  two  kinds  of  inputs  arc  depicted  below: 

1-  For  a  constant  step  input: 

The  constant  inputs  =  5  and  T),.  =0.1  is  applied  to  both  body  and  stability  dynamics 
and  the  outputs  arc  depicted  in  figure  4: 
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15  ^  2 

. Stability, 


Fig  4.  Simulation  results  of  both  Stability  and  body  dynamics  for  constant  inputs 


2-  For  a  periodic  input: 

In  this  case  a  periodic  and  a  constant  is  applied  to  both  body  and  stability 
dynamics;  the  simulation  results  are  depicted  in  figure  5; 


Fig  5.  Simulation  results  of  both  Stability  and  body  dynamics  for  periodic  inputs 


Figure  4  and  5  show  that  the  outputs  of  both  body  and  stability  models  are  the  same  when 
we  apply  the  same  inputs  to  them  and  their  behaviours  are  similar;  this  means  there  is  no  error  in 
dynamics  due  to  conversion  from  body  coordinate  to  stability  coordinate  and  the  stability 
dynamics  24  and  25  can  be  used  to  design  the  receding  horizon  control  (RHC)  for  the  3DOF 
helicopter. 
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4.2.4  Quasi-infinite  Receding  Horizon  Control 

Suppose  that  the  nominal  model  is  presented  as  the  following  general  form: 

X  =  f{xil),u(t))  ■v(0)  =  -r„  (26) 

Where  \{t)  e  is  the  state  of  the  system  and  u{l)  e  tK"'  is  the  input  vector  satisfying  the 
eonstraints: 

u{l)eU  V/  >  0  (27) 

U  is  the  set  of  allowable  inputs.  In  order  to  meet  the  necessary  conditions  for  stability,  it  is 
also  assumed  that; 

Al-  /’is  twice  continuously  differentiable. 

A2-  U  is  compact  and  convex  and  contains  origin. 

A3-  system  (26)  has  a  unique  solution  for  a  given  initial  condition  [7]. 

Then,  under  these  eircumstanees  the  RHC  is  the  repeated  solution  of  the  following  problem 
[9]: 

Problem  1 :  Find 

T 

min  [(;/(.v(r),if(r))r/r  +  F(.v(7')) 

Ut,  J 

Subject  to 

x{t)  =  f{x{t)Mt))  ■v(0)  =  .ro 
/6o  <  v(.r(0),H(0))  < 

Ih,  <  S{x{t),u{t))  ^  i<b, 

x{T)ei\ 

Where 

Q,  -  {v  e  tir  :  F(.v)  <  r) 


(28) 

(29) 

(30) 

(31) 

(32) 

(33) 
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y  is  an  initial  constraint  on  the  parameters  and  S  is  a  trajectory  constraint  enforced  over  the 
entire  time  interval,  /b  and  i/b  arc  the  lower  and  upper  bounds  respectively  on  the  constraints. 

Constraint  (32)  is  added  to  guarantee  the  stability  of  the  RHC;  in  fact,  by  the  means  of  the 
RIIC  the  trajectory  of  system  is  driven  to  a  neighbourhood  of  the  origin  and  after  that  a  linear 
local  feedback  controller  stabilizes  the  system;  hence,  for  the  open  loop  system,  control  input  is 
as  follows: 


i/(s)  = 


\k.x{s) 


xeXr^ni 

X  eQ, 


(34) 


Where,  .r  is  the  set  of  initial  states  that  there  exists  a  feasible  solution  for  them,  Q).  is  the  set 


of  states  which  do  not  belong  toQ,.and  K  is  the  gain  of  local  feedback  controller. 


Remark:  in  the  quasi-infinitc  RHC  the  local  feedback  controller  is  used  in  ofOinc 
computations  to  prove  the  stability  of  the  RHC  scheme  and  design  the  cost  function  penalties;  in 
that,  in  the  online  eomputations  only  the  RHC  controller  scheme  is  used. 

Repeating  this  computation  yields  a  closed-loop  feedback  control  law.  For  a  receding  horizon 
sampling  period  3  when  S  belongs  to  (0,  7];  then,  the  closed-loop  system  is  represented  as; 

^(z')  =  /(-v(r),w‘(r)) 

u"  (t)  -  u‘ {t,x{t))  r&[t,t  +  S),  0<S<T 

Wherei/(r;.r(/)),  re[/,/-F7'],  is  the  optimal  control  obtained  from  the  above  problem 
with  the  initial  condition  x(l),  and  t  is  the  start  time  of  the  optimization  process  and  the  instant  at 
which  states  are  sampled. 

One  suitable  choice  for  cost  function  (28)  is  the  quadratic  cost  functions,  because  of  their 
unique  properties  in  the  optimization  problems  [11].  With  the  following  selections,  the  RHC 
problem  becomes  quadratic: 

^(.v(/),»(/))  =  ^.v"(/)av(/)  +  ^i/(07?»(0  (36) 

K(x(n)  =  ^-v(n"/’.v(7’)  (37) 
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Q,P  e  'jr*"  and  R  e  denote  positive-definite,  symmetric  weighting  matrices,  T  is  a  finite 
prediction  time. 

Since  states  and  control  inputs  implicitly  depend  on  time,  it  is  required  to  parameterize  the 
states  and  control  inputs  of  system  as  a  function  of  time  to  solve  this  optimization  problem.  The 
next  section  discusses  the  B-Splines  as  the  basis  functions  for  parameterization  of  states  and 
control  inputs. 


1.2.1.  B-Spline  Basis  functions 

Polynomials  are  suitable  basis  functions  for  trajectory  generation  since  it  is  easy  to  evaluate, 
differentiate  and  integrate  them.  However,  they  have  some  serious  deficiencies;  for  example,  if  a 
function  is  approximated  in  a  large  interval  then  the  order  of  approximating  polynomial  can  be 
quite  large  [10].  Another  problem  occurs  from  the  global  dependence  on  local  parameters;  in 
that,  if  a  fit  is  poor  in  a  small  area  then  it  will  be  poor  in  whole  curve  [10].  In  that,  if  one  of  the 
polynomial’s  coefficients  is  not  set  correctly  due  to  an  error  in  curve-fitting,  since  this  coefficient 
will  be  used  for  calculating  the  eurve  over  all  points,  then  the  error  will  affect  all  the  curve  fit?; 
one  remedy  is  to  use  B-Spline  basis  functions. 

The  general  idea  in  construction  of  B-Spline  curves  is  to  control  the  trajectory  by  some  control 
points  P,;  figure  6  shows  the  effect  of  a  control  point  on  the  trajectory.  The  control  points  attract 
the  trajeetory;  the  value  of  each  control  point  relative  to  the  value  of  other  control  points 
specifics  how  much  that  control  point  can  attract  the  trajectory. 


P:  control  points,  t:  Break  points 


Efiects  of  Moving  a  Control  Point 


Fig.  6.  B-Spline  and  the  effect  of  control  points 


Some  benefits  of  B-Splinc  basis  functions  motivate  engineers  to  use  them  for  the 
optimization  based  trajectory  generation;  for  example: 
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1-  B-Splinc  offers  smoother  trajeetories  than  other  kinds  of  basis  functions  like 
polynomials. 

2-  Since  B-Splines  are  constructed  by  joining  several  polynomials  with  a  prescribed  level 
of  continuity,  they  are  differentially  continuous  in  the  break  points.  Hence,  we  don’t 
need  to  check  the  continuity  during  the  optimization  process;  and  this  will  reduce  the 
computation  burden  which  is  useful  for  the  on-line  applications. 

3-  The  trajectory  always  stays  within  the  area  specified  by  the  control  points  and  won’t 
exceed  the  maximum  and  minimum  control  point.  Hence,  this  property  can  be  utilized 
to  apply  the  constraints  in  the  optimization  problem  easily.  It  is  important  to  mention 
that  the  number  of  control  points  is  calculated  by  equation  (40)  and  the  value  of  each 
control  points  is  determined  by  solving  the  optimization  problem  for  path  planning. 

B-Spline  is  a  generalization  of  polynomial  curves.  It  is  constructed  by  joining  several 
polynomial  curves  with  a  prescribed  level  of  continuity  between  them.  The  points  at  which  the 
curves  are  joined  arc  called  break  points.  A  non-decreasing  sequence  of  real  numbers  containing 
the  breakpoints  is  required  to  construct  the  B-Splincs  and  they  are  called  a  knot  vector.  The 
number  of  times  a  break  point  occurs  in  a  knot  vector  is  called  the  multiplicity  m,  of  that  break 
point.  The  smoothness  Sj  of  a  breakpoint  provides  the  level  of  continuity  at  a  breakpoint.  Figure 
(7)  shows  the  effect  of  multiplicity  on  the  smoothness  of  the  B-Splinc  curves;  as  it  is  shown  for 
high  multiplicity,  the  B-Spline  curv'c  is  smoother  in  the  break  points. 
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Fig.  7.  B-Splinc  and  the  effect  of  multiplicity 


A  breakpoint  is  sri  times  continuously  differentiable.  The  order  r,  of  the  piecewise 
polynomial  can  be  determined  from  5,  and  w,  for  interior  breakpoints  as: 

1]  =  s.  +  m,  (38) 

The  equation  of  a  B-Splinc  with  prescribed  smoothness  s  and  order  r  can  be  written  as; 


y(o  =Y.  ^ 


(39) 


Where,  m  is  the  size  of  knots  vector  and: 

n  =  m  -  r  - 1  (40) 

Also,  Njjt)  is  the  basis  functions  at  time  t,  and  P,  arc  the  control  points.  Mathematically,  A, is 
computed  from  the  following  recursive  formula: 

t  <t  <t 


N  ,.M  = 


I  -  ■  ■  ■  y  +  i 

0  otherwise 

t  - 


(41) 


^-rr-t-l  ^-t-l 


Interested  readers  arc  referred  to  [10]  for  more  information  on  B-splines. 

An  m-filc  is  created  to  calculate  form  the  above  recursive  procedure;  this  m-file  is  called 
'"spline Jun_l.m".  This  code  is  a  function  that  calculates  NjJt)  and  its  1st  and  2nd  derivatives  at 
a  given  time;  the  inputs  arc  order  of  B-splinc  and  a  given  time  at  which  we  want  to  calculate 
Njrlt).  The  control  points  P,  are  obtained  by  solving  the  optimization  problem. 


4.2.5  Implementation  of  RHC  to  inner  loop  controller  design 

One  of  the  most  efficient  tools  for  decreasing  the  computation  effort  in  optimization 
problems  is  the  fiat  output.  The  main  idea  is  to  reduce  the  dimension  of  the  optimal  control 
problem  to  a  lower  dimension  casing  the  formulation  and  reducing  the  amount  of  required 
computations  for  optimization  process.  The  motivations  for  using  fiat  outputs  in  optimization 
problems  arc  discussed  completely  in  the  previous  reports  [12]  and  interested  readers  are  referred 
to  [1 1]  for  more  detail  information  and  application  of  fiat  outputs  in  the  trajectory  generation  and 
optimization  problems. 
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The  3DOF  helicopter  dynamics  in  stability  coordinate  is  presented  in  (24);  to  apply  RHC  to 
this  dynamics,  3  flat  outputs  arc  required: 

z,  =  .v,  =  u 

'  3,  =  A%  =  p  (42) 

z^  =  x,=y/ 

Then,  all  other  states  and  control  points  can  be  expressed  as  a  function  of  the  flat  outputs  and 
their  derivatives.  Each  of  these  flat  outputs  is  parameterized  by  a  B-Splinc.  The  order  of  B- 
Splinc  is  3,  and  the  break  points  are  the  initial  time  and  finite  horizon  time  {[0,  T]).  It  is  obvious 
that  with  larger  prediction  horizon  time  T,  the  solution  of  RHC  will  get  closer  to  the  optimal 
solution;  on  the  other  hand,  larger  T  will  increase  the  computational  burden.  For  larger  T,  we 
must  define  more  intervals;  hence  this  will  increase  the  number  of  break  points  which  requires 
more  control  points  over  a  wider  interval.  The  optimization  problem  then  has  to  be  solved  over  a 
wider  interval;  consequently  it  will  increase  the  computation  burden.  Hence,  selection  of  T  is  a 
trade  off  between  computation  effort  and  optimality.  For  our  problem  by  a  trail  and  error 
procedure  the  optimal  finite  horizon  time  is  chosen  as  T=1  sec. 

Also,  the  execution  time  J  should  be  as  small  as  possible  to  meet  the  necessary  conditions  for 
stability  and  performance;  however,  in  practice,  it  is  limited  by  the  sampling  time  and  the 
computation  time  required  for  running  the  optimization.  Actually,  the  execution  time  has  to  be 
larger  than  the  sampling  time  to  allow  for  the  communication  among  agents  and  measurements 
by  sensors.  Also,  it  has  to  be  larger  than  the  computation  time  to  allow  the  optimization  process 
be  done  completely. 

In  our  case,  the  execution  time  is  chosen  as  ^  =  0.1  sec;  in  fact,  it  is  assumed  that  the 
sampling  time  and  the  required  time  for  optimization  arc  smaller  than  0.1  sec.  The  matrix 
penalties  of  quadratic  cost  function  arc  chosen  as  below; 


4-21 


Chapler  4:  Fully  Decentralized  Receding  Horizon  Control  (RHC)  Algorithms  for  Cooperative  Path  Planning  and  Trajectory 
Generation  of  Multi-Vehicle  Systems 


Q  = 


1  0 
0  1 
0  0 
0  0 
R  =  0.0000001 
0.00031 
0 
0 
0 


p  = 


0  0 

0  0 

0.001  0 
0  10 
X  I 
0 

13.1705 

0.00000236 

132.4 


0 

0.00000236 

0.00000307 

0.000055 


0 

132.4 

0.000055 

1342.795 


(43) 


Where,  I  is  the  identity  matrix.  P  is  obtained  from  the  Lyapunov  equation  for  the  linerized  of 
helicopter  dynamics,  and  also,  Q  and  R  are  tuned  based  on  a  trial  and  error  procedure  for  the  best 
response  of  system. 


4.2.6  Simulation  Results: 

In  this  section,  the  simulation  results  of  the  RHC  controller  designed  in  the  previous  section 
is  presented  for  different  kinds  of  manoeuvres.  In  all  cases,  since  our  simulations  arc  not  real 
time,  to  investigate  the  feasibility  of  the  controller  in  the  real  world,  the  computation  time,  C 
(optimization)  is  compared  to  the  simulation  time  or  the  time  required  in  the  real  world  to  do  the 
manoeuvre  (fr).  For  example,  if  an  aircraft  aims  to  do  a  turn  manoeuvre  (following  a  circle)  with 
a  turning  rate  of  2  dcg/scc,  it  will  take  180  sec  (C),  but  if  we  want  to  simulate  this  manoeuvre  by 
computer  this  may  take  less  than  180  sec  or  more  (C).  For  the  feasibility,  it  is  desired  that  the 
optimization  time  be  less  than  the  manoeuvre  time  (C  <  /,).  The  simulation  is  done  by  Matlab 
software  and  the  related  code  is  "RHC  3DOFJ{eli_closeloup.m"  that  invokes  two  other 
functions  called:  “jfun _closeloop.nr  and  "spline J'un  l.m".  The  first  function  represents  the  cost 
function  to  be  minimized  by  RHC  and  the  second  one  outputs  the  B-Splinc  basis  function  and  its 
1’’'  and  2"*^  derivatives  to  be  used  for  trajectory  generation.  Complementary  comments  within  the 
m-files  help  the  user  to  understand  each  part  of  the  code.  The  optimization  toolbox  of  Matlab  is 
used  to  solve  the  optimization  problem  of  RHC;  this  toolbox  can  solve  the  optimization  problems 
with  all  kind  of  constraints  including  linear  and  nonlinear  constraints.  However,  this  toolbox 
cannot  be  used  for  real-time  implementation;  thus,  in  the  future,  RT  optimization  codes  such  as 
SNORT  will  be  used  and  thoroughly  investigated. 
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1.4.1.  Stabilization: 

For  the  stabilization  case,  the  simulation  results  arc  depicted  in  figure  8;  the  simulation  is 
done  for  an  initial  disturbance  in  forward  velocity  and  yaw  angle.  As  it  is  illustrated  from  figure 
8,  the  RHC  controller  stabilizes  the  system  properly.  Also,  the  simulation  time  ist^  =5  sec,  and 
the  computation  time  for  this  case  is  =3.89scc  which  is  smaller  than  thc/^,  this  means  that 

the  simulation  can  be  done  in  real  time  applications  and  the  controller  is  feasible.  However,  it  is 
an  estimate  of  the  computation  time  for  the  entire  simulation.  At  each  sampling  instant  the 
computation  time  may  take  longer  than  the  selected  sampling  period. 


Time,  sec 


tc/t^  3.89  sec/  5  sec  =  0.778 


Kig8. Simulation  results  ofRIlC,  inner  loop  control  Tor  stabilizing  case 
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4.2.6.1  Turn  over  manoeuvre  with  radius  =  100  in 

For  the  tracking  case,  different  manoeuvres  are  simulated  to  show  the  effectiveness  of  the 
quasi-infinite  RHC  method.  In  the  first  case,  a  turn  over  manoeuvre  with  radius  of  100  m  and 
angular  velocity  (yaw  rate)  of  =  0.1  rad/Sec  is  considered  and  the  simulation  results  arc 
depicted  in  figure  9.  As  it  is  obvious  from  figure  9,  the  RIIC  controller  follows  the  command 
trajectory  properly:  The  error  in  .v  and  v  positions  is  also  depicted  in  figure  9;  it  is  seen  that  the 
error  in  v  direction  is  more  than  the  error  in  x  direction;  this  is  due  to  the  fact  that  the  helicopter 
dynamics  is  under-actuated  and  in  fact  there  is  no  side  controller. 

Furthermore,  the  simulation  time  in  this  case  isf,^  =  60  sec ,  and  the  computation  time  for  this 
case  is  =42.438sec  which  is  smaller  than  thef^;  hence,  the  simulation  can  be  done  in  real 
time  applications  and  the  controller  is  feasible. 


I  rajectory  following 


...  - 

/  : 

'  'N  : 

/ 

_  '1 

U  !  —  Desired  Trajectory  ^ 

:  -  Actual  Trajectory 

\ 

■>  ' 
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Control  Input 


Fig9. Simulation  results  of  RHC,  inner  loop  control  for  Trajectory  tracking  (Radius  =l00m) 
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1.4.3.  Turn  over  manoeuvre  with  radius  =  25  m 

The  second  tracking  case  is  a  turn  over  manoeuvre  with  radius  of  25  m  and  angular  velocity 

(yaw  rate)  ofru  =  0.05  rad/Sec ;  the  simulation  results  are  depicted  in  figure  10.  As  it  is  obvious 
from  figure  10,  the  RHC  controller  follows  the  command  trajectory  properly.  The  error  in  .v  andy 
positions  is  also  depicted  in  figure  10;  again  in  this  case,  although  the  helicopter  is  under- 
actuated,  the  error  is  in  an  acceptable  range. 

Moreover,  the  simulation  time  in  this  case  is/,.  =  120  sec ,  and  the  computation  time  for  this 
case  is  =89.203sec  which  is  smaller  than  the/,.;  hence,  the  simulation  can  be  done  in  real 
time  applications  and  the  controller  is  feasible. 


Trajectexy  following 


Time,  sec  Time,  sec 


FiglU. Simulation  results  of  RHC,  inner  loop  control  for  Trajectory  tracking  (Radius  =  25  m) 


4-25 


Chapter  4:  Fully  Decentralized  Receding  Horizon  Control  (RHC)  Algorithms  for  Cooperative  Path  Planning  and  Trajectory 
Generation  of  Multi-Vehicle  Systems 


1.4.4.  2D  Spiral  manoeuvre 

In  the  third  tracking  case  a  two  dimensional  spiral  manoeuvre  is  considered  and  the 
simulation  results  arc  depicted  in  figure  11.  As  it  is  shown  in  figure  11,  the  RHC  controller 
follows  the  commanded  trajectory  properly.  The  error  in  x  and  v  positions  is  also  depicted  in 
figure  II;  again  in  this  case,  although  the  helicopter  is  under-actuated,  the  error  is  in  an 
acceptable  range. 

Moreover,  the  simulation  time  in  this  case  isf^  =  200  sec ,  and  the  computation  time  for  this 
case  is  =161.76sec  which  is  smaller  than  the/^;  hence,  the  simulation  can  be  done  in  real 
time  applications  and  the  controller  is  feasible. 


1  .  , 

I  -  error  in  k  j 

I  error  m  y  j 

08-  ‘  -  - - ^ 

06 


04. 


LU 

0^ 


■®®0  50  100  150  200  250 

time,  sec 


tc/t=  161 .76  sec/  200  sec  =  0.808 


FigI  I. Simulation  results  of  RMC,  inner  loop  control  for  Trajectory  tracking 

Simulation  results  show  that  although  the  system  is  under  actuated,  the  RHC  controller  can 
control  it  properly  in  different  situations. 
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4.3  Decentralized  RHC  and  Problem  Scenarios 

In  the  path  planning  of  multi-UAVs  a  trade-off  between  “Decentralization”  and 
“Cooperation”  modes  should  be  adopted  to  achieve  the  best  performance  of  the  path  planner. 
However,  in  some  cases,  depending  on  the  availability  of  the  communicated  information  only 
one  of  these  modes  can  be  used.  A  fully-decentralized  path  planner  doesn’t  use  the 
communicated  information  from  other  UAVs  and  plans  the  path  based  on  an  estimate  of  other 
UAVs  information  (such  as  position).  And  also,  in  a  fully-centralized  path  planner  it  is  assumed 
that  there  is  no  restriction  on  the  communication  and  the  UAVs  can  receive  the  communicated 
information  whenever  they  want  with  any  arbitrary  sampling  rate.  In  fact,  the  path  planner 
doesn’t  need  to  estimate  the  other  UAVs  information  because  it  can  obtain  all  information 
through  communication  with  other  UAVs. 

Depending  on  whether  the  path  planner  uses  the  communicated  information  (cooperation)  or 
the  predicted  information  (decentralization),  the  following  architectures  for  path  planning  can  be 
proposed: 

1-  Fully-decentralized  path  planner  (no  communication) 

2-  Overlapping  decentralized  path  planner 

3-  Overlapping  decentralized  path  planner  with  coordinator 

4-  Fully-centralized  path  planner  (only  communication) 


Fig.  12:  the  effect  of  information  sharing  the  autonomy  of  each  UAV 
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Figure  12  shows  how  the  autonomy  and  cooperation  will  vary  by  choosing  any  of  the 
architectures.  Fully-decentralized  path  planner  architecture  enables  the  UAV  to  be  completely 
autonomous  and  plan  its  own  path  without  communication  with  other  UAVs;  in  a  fully- 
dcccntralizcd  architecture  since  UAV  doesn’t  communicate  with  other  UAVs,  it  needs  an 
estimate  of  other  UAVs  position  to  plan  its  own  path  and  avoid  collision  with  other  UAVs.  Also, 
in  a  fully-centralized  path  planner  the  highest  level  of  cooperation  is  done  among  UAVs  and  the 
lowest  level  of  autonomy  will  happen.  The  second  and  third  architectures  are  a  trade-off  between 
the  first  and  forth  architectures.  Based  on  the  availability  and  reliability  of  the  resources  of 
information  (communication  and  estimation),  any  of  these  four  architectures  can  be  triggered 
during  performing  the  mission.  The  word  “Overlapping”  is  used  because  these  architectures  use 
a  combination  of  communicated  and  predicted  information.  In  this  report  only  the  “Fully 
Decentralized  Path  Planner”  will  be  addressed  completely  and  the  other  three  path  planners  will 
be  discussed  in  the  future. 

In  the  control  of  UAVs  in  a  fully  decentralized  manner,  each  UAV  plans  the  paths 
independently  without  communication  with  other  UAVs;  in  fact,  there  is  no  central  decision 
maker.  The  only  shared  objective  is  the  task  allocation,  done  whenever  it  is  needed;  in  fact, 
communication  is  done  after  doing  every  set  of  tasks  by  UAVs.  For  example,  suppose  that  5 
UAVs  want  to  do  15  tasks;  firstly  5  closest  tasks  arc  assigned  to  5  UAVs  based  on  the  initial 
position  of  UAVs;  then  all  UAVs  plan  the  paths  towards  their  assigned  task;  after  finishing  their 
task  the  UAVs  again  communicate  with  each  other  and  inform  other  UAVs  about  their  new 
position;  again  based  on  the  new  position  of  the  UAVs,  5  tasks  arc  assigned  to  5  UAVs;  this 
procedure  will  be  repeated  till  performing  all  tasks  by  UAVs.  However,  it  is  possible  to  use  the 
communicated  information  efficiently  for  improving  the  path  planning  unit  performance 

In  the  centralized  control  method,  the  computation  and  decision  making  is  done  by  a  single 
UAV  or  a  central  ground  station;  hence,  if  this  UAV  fails,  or  gets  shot  down  by  an  adversary,  the 
other  UAVs  won’t  be  able  to  carry  on  the  mission.  Another  advantage  of  reduced  inter-vehicle 
communications  is  to  lower  the  probabilities  enemy  radars  detect  the  UAVs.  These  benefits  of 
decentralized  control  over  centralized  cooperative  control  have  motivated  researchers  to  pursue 
effective  distributed  solutions. 
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In  figure  1,  the  schematic  block  diagram  of  cooperative  control  for  one  UAV  is  shown.  The 
UAV  and  Controller  blocks  arc  discussed  and  designed  in  chapter  1.  In  this  chapter,  we  will 
work  on  the  Task  Assignment  and  Path  Planning  blocks. 

Each  mission  scenario  consists  of  two  main  parts:  task  assignment  and  path  planning.  The 
task  assignment  part  is  a  decision  process;  in  fact,  based  on  the  minimization  of  a  cost  function, 
one  or  more  targets  will  be  allocated  to  each  UAV.  The  cost  function  can  be  the  sum  of  the 
distances  between  UAVs  and  targets.  In  some  simple  mission  scenarios  where  there  are  no 
moving  and  pop-up  targets,  the  task  assignment  will  be  done  once  at  the  beginning  of  scenario 
and  the  assigned  task  won’t  be  updated  during  mission.  Then,  some  issues  such  as  pop-up  targets 
and  threats  arc  referred  to  the  task  assignment  part;  it  means  when  some  unexpected  targets  arc 
discovered,  the  task  assignment  unit  may  change  the  current  decision  and  assigns  the  discovered 
target  to  the  closest  UAV. 

As  soon  as  the  targets  arc  allocated  to  each  UAV,  the  path  planning  begins.  In  the  path 
planning  block,  it  is  typically  assumed  that  the  UAV  knows  its  own  initial  position,  the  initial 
position  of  some  (or  all)  other  UAVs,  and  the  position  of  the  target  assigned  to  itself  Then, 
based  on  this  primary  information,  path  planning  is  done  under  the  condition  that  UAV  has  to 
visit  the  assigned  target  without  colliding  to  other  UAVs.  One  important  question  here  is  that 
since  there  is  no  communication  possible  among  UAVs,  how  collision  avoidance  between  UAVs 
is  guaranteed  during  flight?  We  will  answer  this  question  later  for  our  proposed  approach.  Some 
problems  such  as  obstacle  avoidance  are  referred  to  path  planning  part;  in  fact,  the  planned  paths 
mustn’t  pass  through  the  obstacles. 

The  path  planning  method  must  be  such  that  the  planned  path  is  updateable  during  the  flight; 
especially,  in  the  case  of  moving  or  pop-up  targets;  because,  in  these  cases,  the  task  assignment 
unit  will  update  its  decision  based  on  the  new  position  of  moving  targets  or  position  of  pop-up 
targets. 

In  this  section,  we  will  introduce  some  of  the  recently  presented  scenarios  in  the  open 
literatures: 

1-  SEAD  (Suppression  of  Enemy  Air  Defences)  1 1  ]: 

The  first  scenario  is  discussed  in  the  research  works  of  West  Virginia  University.  A  simple 
battlefield  is  shown  bellow;  UAVs  are  shown  as  blue  diamonds  numbered  1  through  4  along  the 
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left  side  of  the  battlefield.  The  statie  targets  arc  green  'x’s,  while  the  single  pop-up  target  is 
shown  as  a  green  The  no-fly  zones  arc  the  obvious  black  circles.  Threats  arc  shown  as  a  red 
star  with  surrounding  effective  radius  for  the  static  variety,  and  the  pop-up  threat  is  the  large  read 
range  with  the  red  ‘O’  at  the  center. 
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Fig.  13.  Suppression  of  Enemy  .Air  Defences  scenario 

Given  information; 

1-  rtffav:  number  of  UAVs 

2-  ntargji:  number  of  stationary  targets  or  waypoints  to  be  visited 

3-  ntarg_p:  number  of  pop-up  targets 

4-  nzones:  number  of  no-fiy  zones  (obstacles) 

Goal:  UAVs  must  visit  each  target  while  minimizing  an  overall  cost  to  the  group 

2-  Cincinnati  University  18): 

The  second  scenario  comprises  the  following  elements; 


1  ■■■  . I - - — r--  '  i  - 1 - r 
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1-  A"  heterogeneous  UAVs. 

2-  Mk  stationary  targets;  Mk  are  initially  known 

3-  M),  pop-up  targets;  Mh  is  unknown  a  priori. 

4-  A  bounded  Lx  x  Ly  mission  environment  in  whieh  the  UAVs  operate.  The 

environment  is  represented  in  the  UAVs’  information  bases  as  a  grid  of  eells; 

{(x,y),x  =  \,..,L^,y  =  LJ  (44) 

It  is  assumed  that  eaeh  eell  eontains  at  most  one  target,  and  that  targets  do  not  cross  cell 
boundaries. 


Figl4.  A  typical  multi-U.AV'  Scenario  proposed  by  Cincinnati  University 

Triangles  indicate  UAVs  and  circles  are  targets;  the  blue  region  may  denote  a  lake  with  a 
zero  target  probability  and  the  lighter  green  region  may  indicate  a  camp  with  a  high  target 
probability.  The  mission  of  the  UAV  team  is  to  discover,  identify  and  vcrifiably  destroy  all 
targets  in  an  uncertain  environment.  The  UAVs  do  not  have  access  to  centralized  information, 
and  each  UAV  makes  its  decisions  based  on  subjective  information  obtained  through  observation 
and  communication  with  other  UAVs.  However,  rather  than  operating  separately,  they  cooperate 
in  two  ways:  1)  by  sharing  information  among  the  team;  and  2)  by  coordinating  their  tasks. 

To  perform  this  scenario,  an  algorithm  is  proposed  where  UAVs  autonomously  make 
cooperative  task  allocation  decisions  using  a  common  information  base,  providing  them  with  a 
global  view  of  the  mission.  The  focus  of  this  approach  is  on  the  use  of  very  simple  decision¬ 
making  rules  by  individual  UAVs  rather  than  solving  a  global  optimization  problem. 
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3-  The  Ohio  State  University  |13]: 

The  third  scenario  is  defined  in  the  Ohio  state  university.  There  is  a  group  of  Autonomous 
Air  Vehicles  (AAV)  that  performs  surveillance  of  a  given  area  and  can  use  information  from  a 
satellite  to  pursue  suspected  locations  in  that  area.  The  set  of  AAVs  is  given  a  number  of  targets 
to  search  for,  with  their  respective  likely  locations.  AAVs  must  work  together  autonomously  in 
order  to  try  to  maximize  the  probability  of  finding  targets  in  the  environment  with  minimal  AAV 
effort.  This  cooperation  must  be  performed  in  spite  of  imperfect  inter-vehicle  communications, 
less  than  full  communication  connectivity  between  vehicles,  uncertainty  in  target  locations,  and 
imperfect  vehicle  search  sensors. 

They  decompose  the  search  area  into  NQ  cells.  These  cells  arc  numbered  so  that  the 
discretized  search  space  \%Q  =  {I,  .  .  . ,  NQ}.  It  is  assumed  that  all  the  AAVs  know  how  the  cells 
are  numbered  and  hence  know  NQ.  It  is  also  assumed  that  there  are  NV  AAVs  that  search  for 
targets  and  let  V  =  { 1 ,  .  .  .  ,  NV  }  denote  the  set  of  vehicles. 

It  is  assumed  that  there  are  ND  distinct  valid  stationary  targets  that  the  AAVs  arc  searching 
for  and  let  D  -  { 1,  .  .  .  ,  ND)  denote  the  set  of  targets.  It  is  suppose  that  the  size  of  each  target  is 
considerably  smaller  than  the  size  of  each  cell.  Also,  there  could  be  more  than  one  target  located 
in  one  cell;  however,  it  is  assumed  that  all  targets  inside  the  same  cell  arc  separated  in  such  a 
way  that  they  can  be  detected  by  the  sensors  of  the  AAVs. 
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Simulation  lime:  0 


Fig.  15.  A  t\'pical  multi-UAV  Scenario  proposed  by  Ohio  State  University 

They  represent  the  state  of  the  search  progress  with  a  search  map  and  use  an  invariant  set  to 
model  the  set  of  states  where  there  is  no  useful  information  on  target  locations.  They  also  show 
that  the  invariant  set  is  exponentially  stable  for  a  class  of  cooperative  surveillance  strategies. 

4-  University  of  Leicester,  Leicester,  UK  |14|: 

This  scenario  finds  centralized  trajectories  for  3  UAVs  which  start  from  different  positions 
but  fly  to  the  same  destination.  The  operational  region  is  180  kmx200  km  and  has  10  defence 
units  (radar)  shown  as  circles  in  the  figure  16.  Five  units  arc  of  medium  range  (25  km)  centered 
at  coordinates  (100,100),  (125,  65),  (125,135),  (50,155),  (50,  45),  and  the  rest  arc  of  short  range 
(7  km)  centered  at  (42,102),  (167,182),  (167,127),  (167,37),  (167,77).  The  initial  positions  of 
UAVl,  UAV2,  UAV3  are  (10,  10),  (10,120),  (10,180),  respectively.  All  three  UAVs  start  at  the 
same  time  and  move  towards  a  common  goal  at  (170,100).  The  UAVs  fly  at  an  initial  speed  of 
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200  m/s  and  with  an  initial  heading  angle  of  40°  with  regard  to  the  horizontal.  It  is  required  that 
the  final  speed  of  each  vehicle  when  arriving  at  the  destination  is  100  m/s  and  the  heading  is 
along  the  x-axis;  the  maximum  flight  speed  is  300  m/s.  In  this  scenario  the  finite  horizon  T  is 
chosen  as  25  seconds,  i.e.  ,  the  trajectory  is  evaluated  for  five  time  steps  each  of  5  seconds 
duration  but  only  the  first  control  input  is  implemented. 


Figl6.  A  typical  multi-UAV  Scenario  proposed  by  University  of  Leicester 

The  defence  units  are  modeled  as  squares  containing  the  circular  threat  regions.  The  vehicles 
may  enter  threat  zones  but  will  try  to  leave  them  as  soon  as  possible  to  keep  constraints 
violations  at  a  minimum.  It  has  been  observed  that  maximum  violations  will  occur  when  the 
vehicle  enters  a  threat  zone  at  maximum  speed  and  is  perpendicular  to  the  square  boundary  at  a 
tangent  point  with  the  circle. 

5-  MIT  Scenario: 

In  the  MIT  scenario  a  group  of  hetcrogonous  vehicles  perform  a  variety  of  tasks  in  dynamic 
and  uncertain  environments.  In  this  mission  scenario  a  real-time  coordination  and  control 
algorithms  is  designed  for  teams  of  multiple  UAVs  with  a  focus  on  balancing  performance  with 
improving  computational  scalability,  reducing  communication  requirements,  and  increasing 
robustness  to  errors  in  the  situational  awareness. 
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A  team  of  autonomous  aerial  and  ground  vehicles  arc  coordinated  to  destroy  some  pre- 
spccified  targets;  however,  similar  to  the  previous  mission  scenarios  the  pop-up  targets  and 
threats  are  included  to  the  scenario  that  makes  it  more  general  and  complex  [15,16,17,  18], 


+  objective  •  iclentifiecl  target  O  exclusion  zones 

—  path  plaimed  tnlien  X  possible  target  exploration  region 

Figl7.  A  typical  multi-agent  Scenario  proposed  by  MU'  team 
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4.3.1  Proposed  scenario 

Each  mission  scenario  consists  of  two  main  parts:  task  assignment  and  path  planning.  For 
the  task  assignment  part,  we  assume  that  there  are  N  UAVs,  and  N  targets,  and  UAVs  know  the 
initial  position  of  all  UAVs  and  targets;  in  fact,  all  UAVs  know  the  initial  position  on  other 
UAVs  and  all  targets.  The  task  assignment  unit  will  assign  one  task  to  each  UAV  based  on  the 
minimization  of  an  overall  cost  to  all  UAVs. 

For  the  path  planning  part,  we  assume  that  N  targets  are  allocated  to  N  UAVs.  UAVs  start 
from  some  initial  positions  in  their  workspace  and  have  to  visit  their  assigned  targets  without 
colliding  to  each  other.  It  is  assumed  that  each  UAV  initially  knows  the  lollowing  infonnation: 

Given: 

1  -  N\  Number  of  UAV s 

2-  Initial  position  of  all  UAVs 

3-  N:  Number  of  targets 

4-  Initial  position  of  targets 

Objective:  UAVs  have  to  visit  their  assigned  target  while  minimizing  an  overall  cost 
function. 


Remarkl:  Path  planning  in  a  dense  environment  often  gives  a  non-convex  optimization  [19];  then, 
it  won’t  be  addressed  in  this  report  and  will  be  discussed  in  the  future  works.  By  definition  a  set  S 
in  the  Euclidian  plane  is  called  a  convex  set  if  the  segment  line  joining  any  two  points  of  S  tics  in 
S.  For  example: 
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Hence,  if  we  add  an  obstacle  into  a  convex  set,  this  will  make  the  set  non-convex;  this  means, 
the  convex  optimization  methods  can  not  be  used  for  this  kind  of  problems. 
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4.3.2  Task  assignment 

Although  the  main  focus  of  this  report  is  path  planning,  the  task  assignment  will  also  be 
discussed  shortly  in  this  section.  Because,  since  the  initial  position  of  the  UAVs  and  targets  arc 
generated  randomly  and  they  are  not  predefined  to  be  sorted  in  the  best  order,  a  simple  task 
assignment  unit  is  required  to  allocate  the  targets  to  each  UAV  before  the  path  planning  begins; 
this  cause  the  paths  not  to  intersect  each  other. 

The  cost  for  each  UAV  to  visit  a  particular  target  is  a  function  of  the  path  taken.  In  our 
proposed  approach  an  overall  cost  to  go  for  all  UAVs  is  minimized  and  one  target  will  be 
assigned  to  each  UAV.  In  fact,  all  the  possible  cases  (permutations)  of  vehicles  to  targets  are 
considered  and  a  globally  optimal  permutation  would  be  chosen.  The  task  assignment  results  for 
different  number  of  UAVs  and  targets  arc  shown  in  figure  18  and  the  m-filc  related  to  this 
simulation  is  ''task_assignment.m". 

Task  Assignment  (N=2)  Task  Assignment  (N=3) 


KigI8.  T  ask  assignment  simulation  results  for  different  number  of  UAVs 
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4.3.3  Different  methods  in  path  planning  of  UAV's 

2.3.1.  Graph  based  method 

In  the  graph  based  method,  paths  are  generated  from  a  sequence  of  edges  connecting  vertices 
of  the  graph,  optimal  control,  which  computes  an  optimal  path  based  on  a  cost  function,  and 
finally  virtual  potential  fields,  where  a  simpler,  related  problem  is  solved  to  obtain  the  path. 

In  the  graph  based  method,  a  sequence  of  vertices  is  assigned  to  discrete  points  in  space; 
then,  edges  arc  used  to  connect  these  vertices.  After  that,  costs  arc  assigned  to  each  of  the  edges, 
and  lastly  the  graph  is  searched  for  an  optimal  trajectory.  For  a  simple  graph,  vertices  can  be 
assigned  rectangular  points. 


2.3.2.  Potential  function  methods 

The  second  method  to  UAV  path  planning  is  virtual  potential  fields  and  forces,  as  proposed 
by  Bortoff  [20].  In  this  method,  a  chain  of  masses  connected  to  each  other  by  springs  and 
dampers  represents  a  UAV  path.  Obstacles  to  be  avoided,  such  as  radar  and  threats,  have 
repulsive  force  fields  that  shape  the  path  until  equilibrium  is  reached.  This  method  has  had  the 
smallest  amount  of  research  performed  among  the  other  methods,  though  Bortoff  concludes  that 
the  method  is  quite  promising  for  uniform  radar  field  [1]. 

2.3.3.  Probabilistic  Road  maps 

Probabilistic  roadmap  planning  (PRM)  is  an  efficient  method  to  compute  collision-free  paths 
for  UAVs  [21].  This  method  consists  of  two  phases,  a  building  and  a  query  phase.  The  building 
phase  is  the  construction  of  a  graph  called  ‘roadmap’.  The  nodes  in  the  roadmap  are  collision- 
free  configurations  and  the  edges  linking  the  nodes  are  collision-free  paths.  The  query  phase  is 
finding  a  path  between  an  initial  and  goal  configurations  by  connecting  these  nodes  to  the  road 
map  and  searching  them  for  a  sequence  of  edges  linking  the  two  nodes.  This  method  was 
originally  developed  for  holonomic  robots  in  a  statie  environment;  however,  it  has  been  recently 
applied  to  non-holonomic  robots  with  constrained  kinematics  and  high  degrees  of  freedom  [22]. 
In  some  new  extensions  of  this  method,  a  coarse  roadmap  is  built  during  the  building  phase  and 
further  refined  in  the  querying  phase  with  focus  on  the  area  of  interest,  and  customized  to 
specific  preferences  such  as  maximum  number  of  sharp  turns  [23]. 
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2.3.4,  Optimization  based  methods  (RHC) 

In  this  approach  a  cost  function  consisting  of  a  path  length  or  time  or  fuel  is  minimized  to 
generate  an  optimal  path.  The  optimization  problem  is  subject  to  constraints  of  the  starting  and 
final  aircraft  states  and  a  model  of  the  aircraft;  some  limitations  like  maximum  speed  have  to  be 
applied  to  the  optimization  problem.  The  main  drawback  of  this  method  is  that  it  needs  a  high 
computational  time;  and  it  increases  dramatically  with  the  scale  of  the  problem.  Hence,  to  reduce 
the  computational  time  a  finite  horizon  time  is  used  instead  of  an  infinite  horizon  time. 

Designing  a  whole  trajectory  with  a  planning  horizon  fixed  at  the  goal  is  very  difficult  to 
perform  in  real  time  because  the  computational  effort  required  grows  rapidly  with  the  problem 
size.  This  limitation  can  be  avoided  and  trajectories  in  real  time  can  be  obtained  by  using  a  finite 
receding  horizon  approach  to  form  a  shorter  plan  at  each  simulation  step  that  extends  towards  the 
target  but  does  not  necessarily  reaeh  it. 

In  this  approach,  the  path  is  computed  online  by  solving  the  RHC  over  a  limited  horizon  at 
each  time  step.  The  procedure  is  composed  of  a  sequence  of  locally  optimal  segments.  At  each 
time  step,  the  optimization  is  done  for  T  future  time  intervals,  where  the  length  7’ of  the  planning 
horizon  is  chosen  as  a  function  of  the  available  computing  resources  as  well  as  the  individual 
problem.  Solving  this  optimization  provides  the  input  commands  for  the  T  future  time  steps.  In 
fact,  the  path  optimization  is  done  for  a  finite  period  of  time  and  the  first  portion  of  this  path  is 
used  until  the  next  update  is  available.  Figure  21  shows  a  schematie  representation  of  this 
approach. 
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4.4  Proposed  Approach  for  Decentralized  RHC 

In  this  section,  a  decentralized  RHC  path  planning  approach  is  proposed  to  design  on-line 
cooperative  path  planning  for  a  group  of  UAVs  aiming  to  do  the  proposed  scenario  in  section 
4.3.1.  The  schematic  diagram  of  a  decentralized  control  for  3  UAVs  is  depicted  below: 


Figl9.  Block  diagram  of  decentralized  path  planning  for  multi-U.W's 

As  it  is  seen  from  the  above  diagram,  all  tasks  including  task  assignment,  path  planning  and 
control  of  the  UAVs  are  done  dcccntrally;  and  the  UAVs  need  no  communication  during  flight. 

The  only  information  that  UAVs  need  is  the  "initial  position"  of  other  UAVs;  this 
information  can  be  measured  by  UAVs. 

The  path  planning  method  is  the  optimization  based  (RHC)  method  introduced  in  section 
4. 3. 3. 4.  In  this  method,  an  overall  cost  to  all  UAVs  is  minimized  to  generate  the  paths;  in  fact, 
each  UAV  1-  plans  its  own  path  and  2-  estimates  the  paths  planned  by  other  UAVs;  these  two  is 
obtained  by  minimization  of  a  cost  function  by  each  UAV.  The  cost  function  in  our  case  is  the 
distance  from  the  point  reached  at  the  end  of  planned  horizon  (T)  to  the  target;  this  cost  function 
will  be  discussed  completely  later.  A  mathematical  model  of  this  cost  function  is  used  in  the  path 
planning  unit  to  be  minimized.  The  minimization  of  this  cost  function  yields  the  planned  paths 
and  needs  no  information  about  the  position  of  other  UAVs  during  the  flight.  In  fact,  each  UAV 
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minimizes  this  cost  function  to  plan  the  paths  for  all  UAVs;  but  uses  the  path  associated  with 
itself 

The  reason  beyond  the  fact  that  each  UAV  plans  the  paths  for  other  UAVs  is  to  estimate  the 
position  of  other  UAVs  in  order  to  avoid  colliding  with  other  UAVs;  this  estimation  compensates 
for  lack  of  reliable  information  in  the  mode  of  “Fully  Decentralized  Path  Planning”, 

One  important  question  is  that,  how  one  can  ensure  that  all  UAVs  make  the  same  decision  in 
different  situations?  How  is  it  assured  that  there  is  no  difference  between  the  estimated  paths  and 
the  paths  followed  by  other  UAVs?  How  the  uniqueness  of  the  decisions  is  guaranteed?  The 
optimal  nature  of  the  RHC  answers  this  question.  More  specifically,  since  the  global  minimum  of 
the  optimal  control  is  unique,  if  all  agents  achieve  the  global  minimum  of  an  overall  cost  to  go,  it 
can  be  concluded  that  they  will  make  the  same  decisions  and  they  need  no  communication. 

The  global  minimization  and  achieving  the  global  minimum  do  not  need  the  exchange  of  data 
while  the  mission  evolves;  since  the  initial  condition  and  final  conditions  (target  positions)  are 
known  a  priori  by  UAVs;  then  the  optimization  problem  has  a  unique  global  minimum.  For 
example,  when  in  an  optimization  problem  two  points  arc  given,  the  path  with  minimum  distance 
between  these  two  points  is  the  straight  line  between  two  points  and  this  line  is  unique;  hence,  if 
this  optimization  problem  is  solved  by  several  processors  independently,  all  of  them  will  get  the 
same  answer,  provided  that  they  achieve  the  global  minimum  of  the  optimization  problem) 

The  inner  loop  controller  (figure  19)  is  designed  and  discussed  in  section  4.2.  and  also  the 
task  assignment  is  developed  in  section  4.3.2.  In  this  section,  the  path  planning  unit  is  discussed 
and  finally  a  decentralized  RHC  path  planner  will  be  designed. 


4.4.1  Important  Issues  in  Path  Planning 

Some  of  the  most  important  issues  in  the  path  planning  of  multi-UAVs  are; 

1-  The  paths  have  to  satisfy  the  initial  condition  of  UAVs. 

2-  The  paths  have  to  visit  the  target  positions. 

3-  The  paths  must  be  designed  so  that  the  UAVs  don’t  collide  each  other. 

4-  The  paths  must  be  feasible  (it  has  to  satisfy  maximum  acceleration,  speed  and  tuning 
rate  of  UAVs). 
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The  first  and  second  issues  are  easy  to  satisfy;  all  kinds  of  basis  functions  can  easily  apply 
the  initial  and  final  conditions.  For  example,  in  our  case,  when  we  use  B-Spline  basis  functions 
for  trajectory  generation,  it  is  enough  to  set  the  fist  control  point  to  be  equal  to  the  initial 
condition  for  applying  the  initial  condition.  Then,  we  will  discuss  the  3'‘*  and  4'*^  issues  in  this 
section: 


3.1.1  Collision  avoidance  among  UAVs 

The  collision  avoidance  is  a  challenging  problem  in  the  cooperative  control  field.  In  [17], 
collision  avoidance  between  the  UAVs  is  dealt  with  in  a  way  similar  to  the  case  of  stationary 
obstacles.  At  each  time  step  every  pair  of  vehicles  must  be  a  minimum  distance  apart  from  each 
other  in  all  coordinates.  On  the  other  hand,  since  the  distance  of  two  UAVs  is  a  nonlinear  second 
order  function  of  their  positions,  applying  this  kind  of  nonlinear  constraint  to  the  optimization 
problem  is  demanding  and  time  consuming;  especially  in  situations  where  the  number  of  UAVs 
is  huge.  Moreover,  treating  the  neighbour  UAVs  like  an  obstacle  will  make  the  optimization 
problem  non-convex. 

One  alternative  method  is  the  “potential  function  method”;  this  method  has  been  found  the 
most  attraction  because  of  their  efficiency  and  low  computation  time. 

In  the  potential  field  method,  the  idea  is  to  fill  up  the  UAV’s  workspace  with  an  artificial 
potential  field  in  which  the  UAV  is  attracted  to  the  target  and  is  repulsed  away  from  the 
obstacles  and  other  UAVs.  One  general  form  of  these  functions  is: 


J.  =11 


a,{x.  -  x,f  +  b,{y,  -  y ,)~ 


Where  .v  and  v'  are  components  of  position  and  the  degree  of  importance  of  each  of  them  is 
specified  by  a  and  b.  The  above  potential  function  forms  a  cost  function  for  UAV  /,  and  the 
importance  of  this  cost  function  is  specified  by  matrix  K. 


3.1.2.  Linearization  of  nonlinear  constraints 

The  forth  issue  in  the  path  planning  of  the  UAVs  is  the  feasibility  of  the  generated  paths.  By 
the  feasibility  of  the  paths,  we  mean  that  since  the  trajectories  are  parameterized  in  time,  they 
have  to  satisfy  the  maximum  speed  and  acceleration  of  the  UAVs.  Again,  like  the  distance  the 
overall  velocity  of  the  UAV  is  a  nonlinear  function  of  component  velocities: 
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K.,„  =  <«) 

Although  the  exact  representation  of  this  constraint  would  be  nonlinear,  they  can  be 
approximated  by  linear  inequalities.  The  true  magnitude  constraints  enclose  a  circle  on  the  X-Y 
plane,  as  shown  in  figure  20: 


-1  -0  6  0  0  5  1 

V  /V 


«  mAX 

Fig2U.  Approximation  of  circle  by  polygon  for  linearization  of  nonlinear  constraints 


This  circle  can  be  approximated  by  a  surrounded  polygon;  hence,  the  linear  representation  of 
the  nonlinear  constraint  of  velocity  is: 

V ,,  It:  m  /  ^  v 

— —  cos(  - )  <  1  X  cost  - ) 

M  '  M  M  (47) 


.  ,  In  m  . 
sin(  — — )  + 


m  -  \  M 

Where,  M  is  the  degree  of  polygon;  for  M=4  and  M=J0,  the  polygons  are  depicted  in  figure 
20.  Simulation  results  show  that  M=J2  can  construct  a  good  approximation  to  the  circle. 
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4.4.2  Proposed  RHC  Based  Path  Planning  Method 

Figure  21  shows  the  way  that  the  path  is  generated  by  the  means  of  RHC: 


<S> 


X, 


•  UAV 
®  Target 

- Executed  Path 

- Planned  Path 


Fig2l.  RHC  based  path  planning 

At  each  time  step,  path  is  planned  for  a  finite  horizon  time  T  towards  the  target  and  docs  not 
necessarily  reach  the  target;  the  first  portion  of  this  path  is  executed  during  the  execution  time^  . 
In  figure  21,  the  orange  path  is  the  executed  portion  and  the  green  one  is  the  planned  path.  At  the 
next  time  step,  the  path  planning  begins  from  X{S}  towards  the  target. 

Since  in  the  RHC  scheme  the  future  path  is  updated  after  every  execution  time,  it  can  be  very 
flexible  to  moving  targets  and  obstacles  and  even  pop-up  targets. 

In  the  RHC  based  path  planning,  the  idea  is  to  minimize  a  cost  function  for  generating  the 
paths;  the  type  of  cost  function  is  very  important  for  the  optimization  problem.  One  choice  for 
the  cost  function  is  the  distance  between  the  X(T),  the  position  at  finite  horizon  time,  and  X, ,  the 
target  position  [24]: 

J,  ^d{X{T\X,)  =  {X{T)-  X,Y  P{X{T)- X,)  (48) 

Where,  f*  is  a  positive  definite  matrix  which  is  typically  the  identity  matrix,  A'(7’)is  the 
vector  of  position  at  finite  horizon  time  and  X ,  is  the  position  of  target  (figure  2 1 ). 
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Hence,  in  our  proposed  method,  the  overall  cost  function  is  the  summation  of  (48)  and 
(45)  cost  functions;  the  first  one  is  used  to  generate  the  trajectory  and  the  second  one  is 
used  to  handle  the  distances  between  UAVs.  Then,  the  optimization  problem  for  f'  UAV 
is  formulated  as  follows: 

Optimization  Problem  1: 

Minimize: 


'  I  M 


Min  J,  =  {X{T)- Xy  P{X{T)- X,)+ 

Subject  to:  constraints  on  the  UA  V  dynamics  (Speed  and  Acceleration) 


(49) 


Where  X  is  the  vector  of  position  for  all  UAVs  and  X,  is  the  vector  of  position  for  P’ 
UAV.  N  indicates  the  number  of  UAVs  and  P,  K  and  a  arc  matrix  penalties. 

As  it  is  obvious,  the  information  of  all  UAVs  appear  in  the  cost  function;  therefore,  since 
this  architecture  is  fully  decentralized,  the  information  of  the  other  UAVs  is  predicted  by 
the  means  of  RHC  based  path  planner  and  no  communication  is  required  during 
performing  the  mission;  the  architecture  only  needs  the  initial  position  of  vehicles  and 
targets;  it  is  assumed  that  this  information  is  sent  to  all  vehicles  before  starting  the 
mission. 

All  UAVs  solve  the  above  optimization  problem;  solving  the  above  optimization  problem 
yields  the  planned  paths  for  each  UAV  and  an  estimate  of  other  UAVs  paths.  Each  UAV 
follows  its  own  planned  path  and  keep  the  “estimate  of  other  UAVs  paths”  in  a  buffer  to 
use  that  for  future  estimation  of  other  UAVs  path.  The  following  will  propose  an 
algorithm  for  implementation  of  this  RHC-based  path  planner. 

To  develop  the  idea  some  parameters  should  be  defined.  Suppose  that  it  is  desired  to  plan 
the  paths  for  the  time  interval  [to,  //];  in  fact,  at  time  to,  the  UAVs  arc  at  their  initial 
position  and  aim  to  visit  the  targets  at  time  tf.  The  prediction  horizon  is  shown  by  T.  t, 
indicates  the  computation  time  for  each  prediction  horizon  and  5  shows  the  execution 
time,  t  represents  the  current  time  and  the  index  “A”  is  used  to  show  the  time  step.  The 
following  diagram  shows  the  schematic  representation  of  these  time  notations  for 
UAV  in  the  scheduling  diagram  of  RHC  based  path  planner;  this  timing  sequence  is 
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representative  of  on-line  and  on-board  processing.  The  computations  take  place  during 
for  each  UAV  by  UAV  itself.  All  UAVs  use  this  timing  sequence  for  the  RHC-bascd  path 
planner: 

r . 


h  ti^  h+i 


Fig.  22:  Schematic  diagram  for  fully  decentralized  RIIC  based  path  planner  without  Communication 


This  diagram  represents  the  timing  sequence  of  the  RHC-based  decentralized  path 
planner  excluding  inter-vehicle  communication.  For  simplicity  in  this  section,  we  assume 
that  the  computation  time  here  is  zero.  Taking  into  account  that  the  initial  position  of 
each  UAV  and  also  the  position  of  target  assigned  to  each  UAV  arc  known  to  the  rest  of 
the  group  at  time  to,  the  algorithm  of  path  planning  for  i'''  UAV  is  summarized  as  follows 
based  on  the  timing  sequence  of  figure  22: 


Algorithm!: 

1-  Get  the  position  of  other  UAVs  at  time  t=lo,  k-0  (from  sensing  or  using  the  data 

from  ground  control  station). 

2-  Get  the  position  of  targets  and  information  on  which  targets  assigned  to  each 
UAV  at  time  t=tu  (from  sensing  or  using  the  data  from  ground  control  station). 

4-  Plan  the  path  and  estimate  the  paths  of  other  UAVs  for  time  interval  [4,  4+7]  by 

solving  the  optimization  problem  1. 

5-  Command  the  planned  path  over  the  interval  [4,  4^/]  to  be  followed  by  i'''  UAV. 

6-  Sample  the  position  Xacmau  of  i'''  UAV  at  t=tk^i.  (X  is  the  vector  of  position). 

7-  Calculate  the  position  Xesnmated,  of  other  UAVs  at  using  the  estimated 

paths  of  step  4. 

S-k=k+].  Goto  step  4. 
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This  procedure  is  repeated  till  visiting  the  target  position.  The  next  section  will  address 
the  improved  algorithm.  The  simulation  results  for  this  RHC  based  path  planner  will  be 
presented  in  the  next  section. 


4.4.3  Simulation  Results 


In  the  RHC  based  method  for  path  planning  discussed  in  the  previous  sections,  the  cost 
function  is  the  summation  of  (48)  and  (45)  cost  functions.  Also,  the  method  of  section  3.1.2  is 
used  for  linearization  of  the  maximum  velocity  constraints.  In  this  section,  this  method  is 
simulated  by  Matlab  software  for  different  number  of  UAVs  and  the  related  m-file  is 
"PathJ^lanning.m"  which  invokes  three  other  funetions  called;  ''task_aIloc.m'\ 
''cost_to_go_2.m”  and  "'spline J'un_l.m''.  "task_alloc.m"  is  a  function  that  allocates  the  targets  to 
each  UAV;  "cost_to_go_2.m”  is  a  function  that  forms  the  cost  to  go  for  all  UAVs  form  the 
position  at  the  finite  horizon  time  to  the  targets  position  and  "spline J'un_l.m"  outputs  the  B- 
Spline  basis  function  and  its  U'  and  2"*^  derivatives.  B-Spline  basis  functions  of  degree  3  are  used 
to  generate  the  paths.  The  following  figures  show  the  simulation  results  for  a  finite  time  horizon 
7’=5  Sec  and  different  number  of  UAVs; 
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Fig  23.  Path  planning  simulation  results  for  different  number  of  UAV's  (during  Finite  horizon  time) 
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As  it  is  shown  the  UAVs  won’t  reach  their  assigned  target  in  T=5  Sec;  because,  the 
maximum  velocity  is  limited  to  10  m/Sec.  To  reach  the  targets  we  have  to  repeat  the  RHC  based 
path  planner  till  arriving  the  target  position.  In  the  RHC  based  path  planner  the  trajectory  is 
optimized  for  a  finite  horizon  time  T  and  the  first  portion  of  this  trajectory  will  be  used  during 
the  execution  time  J  until  the  next  update  be  available. 

A  path  planning  is  done  for  T=I0  Sec  and(5  =  3  Sec ,  until  visiting  the  targets  by  UAVs; 
again  B-Splinc  basis  functions  of  degree  3  are  used  to  generate  the  paths;  the  breakpoints  are 
selected  as  {0,  T/.  For  the  potential  function  the  following  matrix  penalties  are  used: 

=  10  X  Ones(N,N) 

(50) 

a  =  b  -  0.\y.  Ones(N,N) 

Where  Ones(N,N)  is  an  N-hy-N  matrix  that  all  its  elements  are  one  and  N  is  the  number  of 
UAVs. 

Simulation  results  are  shown  in  figure  24  for  two  UAVs.  In  this  case  for  continuity  between 
generated  paths,  a  second  order  continuity  (position,  velocity  and  acceleration)  is  imposed  to  the 
optimization  problem  as  the  linear  equalities. 
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Path  Planning  Distances  between  UAVs 


Overall  Velocity  Overall  Accelaration 


Fig  24.  Path  planning  simulation  results  by  RUC  for  two  UAV's 

From  the  above  figure  we  can  see  that  the  UAVs  visit  their  target  finally.  It  is  also  seen  from 
the  above  figure  that  the  UAVs  keep  the  appropriate  distance  and  they  won’t  collide  each  other; 
hence,  the  potential  functions  are  suitable  tools  to  manage  the  distance  between  neighbour 
UAVs. 

Furthermore,  the  time  history  of  velocity  in  figure  24  shows  the  overall  velocity  of  the 
UAVs;  as  it  is  obvious  the  ma.ximum  allowable  velocity  (10  m/s)  won’t  be  exceeded.  The  speed 
limitations  arc  linearized  by  approximating  the  circle  to  polygon.  This  means  this  method  is  very 
reliable  for  applying  the  velocity  constraint.  The  overall  acceleration  is  also  shown  in  figure  24 
for  both  UAVs.  For  more  detail  the  time  history  of  velocity  and  acceleration  components  are 
depicted  in  figure  25; 
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Fig  25.  Path  planning  simulation  results  by  RIIC  for  two  DAVs 
The  simulation  results  for  more  number  of  UAVs  are  done  and  the  results  are  depleted  in 
figures  26  and  27.  In  these  eases  also  it  is  seen  that  the  proposed  receding  horizon  optimization 
based  path  planner  generates  the  paths  properly;  the  UAVs  don’t  collide  each  other  and  the 
maximum  velocity  isn’t  exceeded. 

Moreover,  in  all  cases,  it  is  seen  that  the  overall  computation  time  is  considerably  smaller 
than  the  simulation  time;  the  maximum  //,.  won’t  exceed  0.5  though  the  path  planning  is  done 
for  a  quite  large  horizon  T=10  Sec  ( is  the  computation  time  and  is  the  simulation  time  or  the 
amount  of  time  that  the  mission  will  take  in  the  real  time  implementation). 
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Kig  26.  Path  planning  simulation  results  by  RIIC  for  three  L'AVs 
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Fig  27.  Path  planning  simulation  results  by  RHC  for  five  UAV's 
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4.5  Conclusion  and  Future  Work 

4.5.1  Conclusion 

In  this  report  the  fully  decentralized  control  of  multi-UAVs  has  been  studied;  the  problem  is 
divided  into  three  main  parts:  inner  loop  controller  design,  task  assignment  and  path  planning. 
The  3DOF  helicopter  model  is  used  as  the  UAV  model;  also,  the  receding  horizon  control  (RHC) 
is  used  to  design  an  inner  loop  controller  for  the  3DOF  helicopter  model  in  the  stability 
coordinate.  Using  B-Spline  as  the  basis  function  for  optimization  problem,  simulation  results 
show  that  RHC  can  generate  a  feasible  control  action  and  it  can  control  the  nonlinear  and  under- 
actuated  helicopter  dynamics  properly. 

For  the  task  assignment  and  path  planning  parts,  the  main  objective  is  to  use  a  method  which 
needs  minimum  communication  among  UAVs.  The  advantages  of  optimization  based  methods 
have  been  used  to  achieve  this  objective;  the  key  idea  is  that  a  cost  function  is  given  to  all  UAVs 
and  based  on  the  minimization  of  this  cost  function  the  UAVs  make  their  decisions  regarding  the 
task  assignment  and  path  planning.  Since  the  global  minimum  of  this  cost  function  is  unique  for 
all  UAVs,  the  UAVs  will  make  the  same  decision. 

An  optimization  based  task  assignment  unit  is  designed  to  allocate  the  targets  to  each  UAV. 
After  that,  a  receding  horizon  path  planner  is  designed  in  a  decentralized  fashion;  simulation 
results  for  both  task  assignment  and  path  planning  units  show  that,  the  receding  horizon 
optimization  based  method  can  successfully  control  the  fleet  of  UAVs  for  performing  the 
cooperative  missions.  The  only  information  that  UAVs  need  is  the  initial  position  of  other 
UAVs. 


4.5.2  Future  work 

For  the  future  works,  it  is  proposed  to  work  on  the  following  scenarios;  the  first  one  is 
proposed  in  section  4.2.1;  we  can  add  some  obstacles  to  this  scenario.  This  mission  scenario  is 
schematically  shown  in  figure  28: 
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Fig. 28.  Proposed  mission  Scenario  for  future  works 


N  UAVs  start  from  some  initial  positions  (blue  circles)  and  have  to  visit  the  targets  (red 
crosses),  without  colliding  to  each  other  and  obstacles  (black  squares)  and  they  shouldn’t  fly  in 
non-fly  zones  (black  circles)  where  there  is  a  probability  for  existence  of  enemy  radars. 

In  this  report,  the  “fully  decentralized  path  planner”  is  discussed;  for  the  future  it  is  proposed 
to  work  on  the  “overlapping  decentralized  path  planner”  so  that  the  communicated  information 
can  be  used  efficiently  to  improve  the  performance  of  path  planning.  Also,  it  is  desired  to  limit 
the  communication  to  only  neighbour  UAVs  and  not  all  UAVs.  In  fact,  the  communication  is 
done  locally  among  the  neighbour  UAVs  and  not  all  UAVs. 

Also,  the  certainty  level  of  information  can  be  reduced  in  order  to  study  and  increase  the 
robustness  of  the  decentralized  method.  Since  in  the  real-world  applications,  environmental 
information  often  is  known  with  a  limited  level  of  certainty  and  also  the  environment  is  dynamic, 
this  can  make  the  scenario  more  realistic. 
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4.6  Appendix  A:  Flat  Outputs 

The  dimension  of  the  optimal  control  problem  is  reduced  to  a  lower  dimension  that  casing 
the  formulation  and  reducing  the  amount  of  required  computations  for  optimization  process  by 
means  of  so-called  flat  outputs.  For  applying  the  RHC  method  and  solving  the  optimal  control 
problem  the  flat  output  advantages  has  been  utilized.  System  (26)  is  called  a  flat  system  if  there 
exist  outputs  2  [1 1]: 


Z=g(x.u)  (51) 

Such  that  the  states  and  the  control  signal  can  be  recovered  from  z  and  its  derivatives;  that 
is,  all  states  and  control  inputs  can  be  presented  as  a  function  of  flat  outputs  and  their  derivatives: 

(.r,n)  =  /7(z,i,....,z‘^')  (52) 

Equation  (52)  is  only  required  to  hold  locally  [11].  For  a  system  being  flat,  it  is  required 
that  all  system  behaviours  including  states  and  control  inputs  can  be  recovered  from  a  finite 
number  of  flat  outputs  and  their  derivatives  and  without  integration  of  the  flat  outputs;  hence,  h 
has  to  be  a  smooth  function.  The  interested  readers  are  referred  to  [11]  for  more  details.  If  the 
dimension  of  z  is  smaller  than  that  of  x  and  u,  the  optimization  problem  is  mapped  to  a  lower 
dimension. 
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Abstract 


This  report  documents  the  work  related  to  tasks  #7,  8  and  9  for  the  project  entitled 
“Synthesis  and  Implementation  of  Single  -  and  Multi-vehicle  Systems  Guidance  Based 
on  Nonlinear  Control  and  Optimization  Techniques”.  This  report  describes  the  design  of 
a  real  time  scheduling  algorithm  for  multiple  uncertain  receding  horizon  control  systems. 
Experimental  verification  has  been  performed  using  a  set  of  3DOF  miniature  under 
actuated  hovercrafts.  This  demonstrates  the  pcrfonnancc  and  feasibility  of  the  proposed 
method.  The  performance  of  the  method  is  also  demonstrated  by  simulating  it  in  Matlab. 
Furthermore,  an  RHC  Object  Oriented  Library  (RFICOOL)  developed  in  the  CIS  lab  is 
described  in  detail.  This  library  greatly  simplifies  the  implementation  of  scheduled  RHC 
hard  real-time  simulations  and  experimental  implementations  for  multiple  UAV  systems. 
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5.1  Introduction 

Scheduling  of  RUC  computational  and  communication  is  very  important  for 
decentralized  RUC  control  since  it  allows  optimization  of  resources  to  achieve  maximum 
performance  in  the  presence  of  uncertainty,  disturbances,  and  delays.  The  tasks  addressed 
in  this  report  related  to  the  project  entitled  “Real-time  Scheduling  of  Multiple  Uncertain 
Receding  Horizon  Control  Systems”  are  as  follows: 

Task  7:  Design  a  baseline  run-time  scheduling  algorithm  that  provides  connections 
with  constrained  optimization  performance. 

Task  8;  Demonstrate  feasibility,  performance  through  Matlab  and  via  single  and  two 
processor  computing. 

Task  9:  Document  results,  theory  and  practice 

For  these  tasks  we  designed  a  baseline  run-time  scheduling  algorithm  that  provides 
connections  with  constrained  optimization  performance  (task  #7).  We  extended  and 
improved  the  results  of  the  CIS  lab  on  scheduling  of  multiple  RIIC  computation 
problems.  In  this  work,  static  scheduling  algorithms  for  a  single  processor  that  indirectly 
optimize  robust  RHC  performance  are  introduced  to  achieve  good  robust  performance. 
This  is  achieved  through  minimization  of  the  difference  between  the  nominal  and  actual 
models  subject  to  scheduling  constraints. 

Feasibility  and  performance  of  the  proposed  method  was  demonstrated  through  Matlab 
and  also  by  applying  the  method  to  some  sets  of  3DOF  miniature  under  actuated 
hovercrafts.  The  latter  was  done  first,  by  using  two  hovercrafts  with  both  one  and  two 
processors  and  second  by  using  four  hovercrafts  with  2  processors  (task  #8). 

In  order  to  be  able  to  apply  the  RHC  method  to  the  miniature  hovercrafts,  and  then  apply 
scheduling  algorithm  on  them,  first  a  model  of  those  hovercrafts  should  be  discovered 
and  the  parameters  of  that  model  have  to  be  identified.  These  tasks  have  been  done  and 
the  results  are  shown  in  this  report.  The  following  sections  arc  included  in  this  report. 

In  section  two,  real-time  scheduling  of  multiple  uncertain  RHC  systems  has  been 
developed.  In  this  section,  a  new  scheduling  approach  is  proposed  that  considers  the 
effect  of  modeling  uncertainty  for  multiple  continuous  time  RHC  systems.  This  is 
accomplished  by  combining  a  scheduling  approach  with  results  from  continuous  time 
nonlinear  systems  theory.  Assuming  the  prediction  horizon  is  a  known  constant  and  using 
a  single  computing  resource,  a  new  technique  is  proposed  for  determining  the  execution 
horizon  for  multiple  RHC  systems.  The  problem  of  determining  the  execution  horizon  for 
each  subsystem  while  optimizing  the  perfonnance  is  cast  into  a  constrained  optimization 
problem. 
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In  section  three,  application  to  3DOF  under-actuated  miniature  hovercraft  problem  has 
been  investigated.  Since  RHC  approach  is  a  model-based  algorithm,  modeling  and 
identification  of  the  apparatus  has  been  done.  A  Receding  Horizon  Control  Object 
Oriented  Library  (RHCOOL)  has  been  developed  in  C++.  This  library  has  the  benefit  of 
applying  different  RHC  approaches  in  a  hard  real-time  environment  to  multi  UAVs, 
hovercrafts,  and  etc.  Finally,  using  the  RHCOOL,  the  real-time  scheduling  of  hovercraft 
has  been  investigated  based  on  the  designed  scheduling  method. 

In  section  four,  experimental  results  have  been  reported  for  miniature  hovercrafts.  The 
results  are  from  implementing  RHC  approach  to  single  hovercraft  as  well  as  scheduling 
results  of  one  processor  with  two  hovercrafts. 

Conclusions  and  future  work  arc  presented  in  section  five  and  references  are  in  section 
six. 

Section  2  and  section  3.2  of  this  report  arc  describing  Task  #7  and  the  other  sections  of 
the  report  arc  describing  Task  #8  of  the  first  proposal. 
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5.2  Real-time  Scheduling  of  Multiple  Uncertain  RHC  Systems 


This  section  describes  the  scheduling  approach  recently  developed  at  the  CIS  lab  [1], 
Receding  Horizon  Control  (RHC),  also  known  as  Model  Predictive  Control  (MPC),  was 
first  introduced  in  the  process  control  community.  It  has  attracted  the  attention  of  many 
researchers  due  to  its  ability  to  handle  constraints  on  the  states  and  inputs  in  multi¬ 
variable  control  problems  [2],  RHC  is  essentially  a  repeated  on-line  solution  to  a  finite- 
horizon  open-loop  optimal  control  problem.  Based  on  the  currently  available  state  values, 
an  optimal  control  problem  is  solved  for  a  period  of  time  called  the  prediction  horizon. 
The  first  part  of  the  computed  optimal  control  input  is  applied  to  the  plant  in  a  period  of 
time  called  the  execution  horizon  until  the  next  sample  of  the  state  becomes  available, 
where  again  the  same  procedure  is  repeated.  Therefore,  the  execution  horizon  can  be 
considered  as  the  closed  loop  sampling  period'  for  the  RHC  system  and  the  two  terms 
used  interchangeably. 

In  the  past,  the  computational  cost  of  the  repeated  on-line  optimization  problems 
associated  with  RHC  limited  its  applicability  to  relatively  slow  dynamic  systems  such  as 
chemical  processes.  However,  with  recent  advances  in  computing  performance  and 
distributed  computation,  RHC  can  now  be  applied  to  plants  with  fast  dynamics  including 
aerospace  systems.  In  [5],  an  efficient  direct  method  for  solving  optimal  control  problems 
has  been  proposed  based  on  the  properties  oi  flat  outputs.  The  method  proposed  in  [5] 
was  applied  successfully  to  a  vector  thrust  flight  experiment  in  [6].  A  number  of  methods 
have  been  proposed  to  provide  closed-loop  stability  for  RHC  systems  where  a  tcmiinal 
cost  or  terminal  constraint  is  introduced  into  the  optimization  problem.  For  a  detailed 
survey  the  reader  is  referred  to  [3].  Methods  to  improve  the  performance  of  RHC  in  the 
presence  of  computational  delays  have  also  been  recently  developed  [21]. 

Application  of  RHC  to  control  problems  with  multiple  subsystems  such  as  multiple 
Unmanned  Ariel  Vehicle  (UAV)  systems  can  be  addressed  by  applying  RHC  to  the 
individual  subsystems,  which  may  contain  more  than  a  single  CPU.  This  results  in 
multiple  RHC  processes  that  must  be  scheduled  in  an  appropriate  manner  to  achieve 
optimal  performance  in  the  presence  of  computing  resource  limitations  on-board  each 
vehicle  and  across  networked  computing  devices.  Systematic  methods  for  scheduling 
RHC  systems  are  typically  not  discussed  in  the  literature,  despite  the  fact  that  1) 
computational  delays  resulting  from  scheduling  can  dramatically  affect  stability  and 
performance,  and  2)  significant  decrease  in  computing  time  could  potentially  be 
achieved.  The  authors  in  [7]  considered  the  scheduling  of  multiple  discrete  time 
decoupled  linear  RHC  systems  with  a  single  computational  resource.  For  this  case  a 
dynamic  priority  assignment  policy  is  proposed  that  gives  higher  scheduling  priority  to 
RHC  subsystems  with  larger  current  cost  function  values.  Premature  termination  of  the 
optimization  processes  is  also  employed  to  maximize  nominal  performance  in  the 


'  Sometimes  the  execution  horizon  maybe  considered  bigger  than  one  sampling  period.  For  example  in  this 
report  when  two  apparatus  are  scheduled  using  one  computer,  the  sampling  period  is  less  than  the  execution 
horizon  of  each  subsystem. 
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presence  of  computational  delays.  These  results  provide  a  new  RHC  scheduling  approach 
for  increased  nominal  performance  of  discrete  time  systems  without  explicitly  accounting 
for  uncertainty  in  the  subsystems. 

In  this  section,  a  new  scheduling  approach  is  proposed  that  considers  the  effect  of 
modeling  uncertainty  for  multiple  continuous  time  RHC  systems.  This  is  accomplished 
by  combining  a  scheduling  approach  with  results  from  continuous  time  nonlinear  systems 
theory.  Assuming  the  prediction  horizon  is  a  known  constant  and  using  a  single 
computing  resource,  a  new  technique  is  proposed  for  determining  the  execution  horizon 
for  multiple  RHC  systems^.  A  rate  monotonic  priority  assignment  [8]  is  employed  due  to 
the  fact  that  it  is  an  optimal  policy  in  the  sense  that  the  task  set  is  not  schcdulablc  using 
any  static  priority  assignment  method  if  it  is  not  schcdulablc  using  the  rate  monotonic 
method  [9],  [10].  The  problem  of  determining  the  execution  horizon  for  each  subsystem 
while  optimizing  the  performance  is  cast  into  a  constrained  optimization  problem.  The 
schedulability  condition  is  represented  by  a  constraint  and  the  robust  performance  is 
formulated  as  the  objective  function.  This  section  can  be  regarded  as  the  first  systematic 
approach  for  scheduling  and  execution  horizon  selection  of  multiple  uncertain  RHC 
systems  subject  to  computing  resource  limitations.  The  new  method  is  applied  for  a  two 
subsystem  example  problem  demonstrating  the  effectiveness  of  the  approach. 


5.2.1  Receding  Horizon  Control  of  Nonlinear  Systems 

The  class  of  systems  considered  in  this  report  is  described  by  the  set  of  equations 
X  =  f{xit),n(t))  .v(0)  =  .v^  (1) 

where  .v(/)  e  'iK”  is  the  state  of  the  system  and  u{t)  e  '.H"’  is  the  input  vector  satisfying 
the  constraints 

Hit)  \/i>0  (2) 

where  V  is  the  allowable  set  of  inputs.  Furthermore,  it  is  assumed  that  (A1-A3)  in  [11] 
arc  also  satisfied;  that  is, /is  twice  differentiable,  U  is  compact  and  convex,  and  system 
(1)  has  a  unique  solution  for  a  given  initial  condition.  Receding  horizon  control  is  the 
repeated  solution  of  the  following  problem. 

Problem  1 :  Find 


■  In  applying  RHC  to  a  system  in  real-time,  periodic  function  is  defined  and  the  period  of  that  periodic- 
function  is  equal  to  the  execution  horizon.  In  scheduling  of  multiple  RHC  systems,  multiple  periodic- 
functions  need  to  be  defined.  The  important  issue  is  to  define  the  period  of  each  function  which  is  equal  to 
the  execution  horizon  of  each  system.  After  defining  the  execution  horizon  of  each  subsystem,  the  priority 
of  each  task  is  defined  by  using  Rate  Monotonic  Priority  Assignment. 
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j‘(Ar(0)=  min  J(j£:(f),H(.),7’) 
«(.) 


(3) 


with 

J{x(t),u{.)J)  =  +||«(r)||Jj):/r  +  ||A-(/  +  7;A:(0)||p 


subject  to 


.v(.v)  =  /(x(.V),M(i-)) 
u(s)  e  U 


s  e  [t,t  +  T] 


(4) 


(5) 


Q  e  'jr’'”  and  R  e  denote  positive-definite,  symmetric  weighting  matrices,  T  is  a 
finite  prediction  horizon  and  x{t\Xg)  denotes  the  trajectory  of  the  system  (1)  driven  by 
u(t)  starting  from  the  initial  condition  xp.  Furthermore,  the  weighted  norms  in  (4)  arc 
defined  as  ||x||^  =  x^Qx  . 

Let  denote  the  RHC  execution  horizon,  where  d'lics  in  the  (0,7]  interval.  The  closed- 
loop  system  is  described  by 

x(r)  =  /(x(r),M*(r)) 

li {t)  =  u  {t\ x{t !.))  T  0  <  (3’ <  7 

where  u‘(r;x(/^)),  re[/j,/^-f7]  is  the  optimal  control  with  initial  condition  x(tk),  tk 
being  the  start  time  of  the  optimization  process  and  the  instant  at  which  states  arc 
sampled.  The  subscript  k  in  4  denotes  the  sampling  time  and  therefore  . 

The  optimization  problem  described  above  can  be  solved  numerically  online  using  a 
number  of  techniques  [6].  Details  of  the  solution  procedure  for  this  section  arc  described 
in  section  2.3.  Numerous  methods  have  been  suggested  to  guarantee  the  stability  of  the 
closed-loop  system  by  requiring  a  terminal  constraint  or  a  special  way  to  select  the 
terminal  cost  [3],  [11].  Note  that  the  RHC  method  can  be  formulated  in  discrete  time  or 
continuous  time  as  is  the  case  for  this  report.  Discretization  is  done  by  zero  order  hold 
sampling  of  the  continuous  time. 


5.2,2  Real-time  Scheduling  of  Multiple  Uncertain  RHC  Systems 
5.2.2. 1  Real-time  Computation  of  RHC  Systems 
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The  open-loop  RHC  optimal  control  problem  can  be  solved  in  a  periodic  manner  with  a 
period  of  S.  The  sampling  instant  is  denoted  by  4  and  the  next  sample  time  is  given  by 
^+1  =?*  A  number  of  parameters,  which  arc  important  for  practical  implementation 
of  the  RHC  algorithm,  arc  defined  below  (refer  to  Figure  ): 

•  Computation  start  time,  tsP-  The  time  at  which  the  optimization  process  is  started. 

•  Computation  end  time,  tec’-  The  time  at  which  the  optimization  process  is  finished, 

•  Computation  time,  Sp.  The  duration  of  the  optimization.  S^. 

•  Actuation  time,  ta'-  The  time  at  which  the  generated  control  input  is  applied  to  the 
system.  This  input  might  be  recently  computed  or  generated  in  advance  depending 
on  the  implementation  method  used. 

•  Actuation  latency,  ly  The  delay  involved  in  the  actuation  of  the  plant  after  the 
sampling  of  the  states. 

5 

/„ 


I 

I 


tk  i  H  I  ec  t  „  tk*! 

Figure  1-  Schematic  diagram  for  RHC  parameters 

In  theoretical  developments  of  RHC,  the  computation  time  is  typically  assumed  to  be 
zero  [11],  [12],  i.e.  5^=0.  However,  in  practical  real-time  control  systems  this  is  not  a 
valid  assumption,  especially  for  systems  with  fast  dynamics.  In  such  systems  {e.g.  sec 
[6]),  unlike  slower  traditional  process  control  problems,  a  small  execution  horizon 
(sampling  period)  is  needed  for  satisfactory  performance  of  the  system.  This  leads  to  the 
situation  where  the  computation  time  is  not  negligible  compared  to  the  execution  horizon, 
and  a  zero  computation  time  assumption  is  invalid.  There  arc  two  methods  available  to 
apply  RHC  to  practical  systems  when  the  computation  time  is  not  negligible.  These  arc 
referred  to  as  Retarded  Actuation  and  On-the-Fly  Computation. 

In  the  Retarded  Actuation  approach,  for  the  control  system  to  have  sufficient  time  for 
generating  the  optimal  trajectories,  the  optimization  problem  is  solved  one  sampling  time 
in  advance  ([6]).  The  control  signal  can  also  equivalently  be  actuated  one  sampling  time 
later.  The  implicit  assumption  of  the  method  is  that  the  computation  time  is  less  than  the 
sampling  period  S.  The  schematic  diagram  for  this  method  is  shown  in  Figure  2.  There 
are  two  variations  of  this  method: 

(i)  The  control  signal  generation  for  the  interval  [tk+i,tk*2\  starts  at  tk,  tk  being  the  A'*’ 
sampling  time  and  tk^i=tk+5.  The  open-loop  optimal  control  problem  is  solved  using  .v(/*) 
as  the  initial  condition.  The  part  of  the  optimal  input  u  corresponding  to  the  time 
interval  [S,2d]  is  applied  to  the  system. 
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(ii)  In  this  variation,  instead  of  using  x{tk^i)  is  predicted  using  the  nominal  model. 
The  predicted  initial  conditions  arc  then  used  for  solving  the  open-loop  optimal  control 
problem.  Contrary  to  the  previous  method,  the  part  of  the  optimal  input  »*  corresponding 
to  the  time  interval  [0,(5]  is  applied  to  the  system. 

The  following  properties  hold,  in  tenus  of  the  parameters  defined  earlier,  for  the  two 
methods  described  above 


Figure  2-  Schematic  diagram  showing  the  Relarded  Actuation  method.  The  implemented  input  is 
shown  by  a  solid  line.  The  subscript  refers  to  the  interval  in  which  the  input  profile  has  been 

generated. 

For  the  On-the-Fly  Computation  approach,  there  is  no  prediction  or  delay  in  the  actuation 
of  the  input  signal  {i.e.  /a=0).  After  sampling  the  states,  optimization  starts  and  as  soon  as 
the  solution  is  found  the  control  signal  is  applied.  While  the  optimization  is  running,  the 
optimal  control  input  from  the  previous  solution  is  being  implemented.  Since  the 
prediction  horizon  is  normally  much  longer  than  the  execution  horizon,  the  method  can 
continue  implementing  the  previously  computed  control  input  until  the  new  input  is 
generated.  Therefore,  the  actual  implemented  input  profile  is  composed  of  two  parts:  The 
continuation  of  the  optimal  input  available  from  the  previous  sampling  and  the  newly 
generated  optimal  input.  This  is  shown  in  the  schematic  diagram  depicted  in  Figure  3. 
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Figure  3-  Control  signal  when  the  On-the-Fly  Computation  method  is  used 
The  following  properties  hold  for  this  method 
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This  report  employs  the  Retarded  Actuation  method  since  it  allows  more  deterministic 
behaviour  than  the  On-the-Fly  Computation  approach  used  in  [7],  This  is  helpful  when 
analyzing  the  effect  of  computational  delays  using  nonlinear  systems  theory  and 
combining  the  approach  with  existing  scheduling  theory.  The  On-the-Fly  Computation 
approach  potentially  provides  better  nominal  performance,  but  it  is  more  difficult  to 
analyze  in  a  deterministic  framework.  Furthermore,  new  methods  have  been  proposed  by 
the  authors  recently  to  improve  the  performance  of  the  Retarded  Actuation  method  using 
neighbouring  extremal  paths  theory  [21]. 

As  discussed  in  [7],  the  delay  introduced  in  actuating  the  plant  due  to  the  computation 
time  can  degrade  the  performance  of  the  system  considerably.  For  this  reason  it  is 
desirable  to  schedule  the  computation  to  improve  the  performance  of  RllC  systems. 


5. 2. 2. 2  Scheduling  Approach  for  Multiple  Uncertain  RHC  Systems 


In  this  section  a  systematic  scheduling  approach  is  proposed  to  determine  the  execution 
horizon  for  each  individual  subsystem  when  the  Retarded  Actuation  method  (type  ii. 
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section  2.2.1)  and  rate  monotonic  priority  assignment  arc  employed.  It  is  assumed  that  the 
optimization  associated  with  the  RHC  problems  converges  in  a  bounded  computational 
time  so  that  the  computation  can  be  scheduled  in  a  hard  real-time  environment.  It  will  be 
shown  that  in  order  to  maximize  the  overall  performance  while  the  schedulability 
property  holds,  a  constrained  optimization  problem  must  be  solved.  Schedulability 
implies  a  guarantee  that  the  optimal  trajectory  generation  for  each  subsystem  using  a 
single  computer  is  finished  before  the  deadline. 

Trajectory  generation  for  multiple  systems  using  a  single  computing  resource  can  be 
considered  a  resource  allocation  problem  described  in  computer  science  literature.  The 
following  definitions  from  this  area  will  thus  be  reviewed  before  addressing  the  main 
problem  of  this  report  [30]. 

Definition  1.  The  time  required  to  complete  a  task  {e.g.  solving  the  optimization  problem 
in  our  case)  is  called  the  execution  time  (sec  Figure  4). 

Remark  1.  The  execution  time  should  not  be  confused  with  the  execution  horizon 
defined  for  RHC  systems.  Execution  time  is  the  same  as  the  computation  time,  Sej, 
defined  in  Section  5.2.2  when  the  trajectory  generation  problem  is  considered. 

Definition  2.  Consider  a  task  /  that  should  be  executed  every  p,  seconds.  This  task  is 
defined  as  a  periodic  task  with  a  period  of /?,  (sec  Figure  4). 

The  period  can  be  fixed  or  time-varying.  In  this  report  only  tasks  with  fixed  periods  arc 
considered. 


Definition  3.  The  CPU  utilization  factor,  //,  for  a  scries  of  n  tasks  is  defined  as  follows 


,^1  Pi 


(9) 


where  <X,,  and  pi  represent  the  execution  time  and  period  of  task  /.  For  a  single  computing 
resource  p  must  be  less  than  one. 

Definition  4.  The  execution  of  task  /  should  be  finished  before  a  certain  pre-determined 
time.  This  pre-determined  time  is  called  the  deadline^  of  task  /  (sec  Figure  4). 

Definition  5.  A  scries  of  tasks  arc  called  schedulable  if  all  tasks  can  be  executed  while  no 
deadline  has  been  missed. 


’  The  deadline  is  equal  to  the  period  for  the  periodic  tasks.  In  addition,  defining  periodic  tasks  is  a  common 
way  for  real-time  operation  of  control  tasks  [35]. 
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Period=p 

/ 

execution  time 


Time 

dead  line 

Figure  4-  Schematic  diagram  for  a  periodic  task 

Derinition  6.  A  task  is  called  preemptive  if  its  execution  can  be  interrupted  by  the 
execution  of  a  higher  priority  task. 

Definition  7  (Rate  Monotonic  Priority  Assignment)  [8],  Given  a  set  of  preemptive 
tasks  with  static  priorities,  the  higher  priority  is  assigned  to  the  task  with  the  smaller 
period. 

Remark  2.  The  following  inequality  is  a  sufficient  condition  to  guarantee  schcdulability 
for  a  set  of  n  tasks  [8] 

"  S  - 

A  =  i;— <«(2''-l)  (10) 

/=1  p, 

The  above  is  a  sufficient  but  not  necessary  condition.  In  other  words,  a  task  set  that  does 
not  satisfy  the  above  inequality  can  still  meet  all  deadlines  using  Rate  Monotonic 
Scheduling  (RMS)  [39].  Rate  Monotonic  Scheduling  is  optimal  under  the  above 
condition  (inequality).  RMS  has  a  nice  advantage.  Also,  in  many  situations,  up  to  90% 
utilization  is  possible  with  RMS  [39].  In  order  to  get  more  information  about  the  different 
schedulability  conditions  sec  [40]  and  [41]. 

The  rate  monotonic  method  will  be  employed  in  this  report  for  static  priority  assignment 
in  scheduling  a  set  of  tasks.  This  approach  is  selected,  among  numerous  other  methods, 
due  to  the  fact  that  rate  monotonic  priority  assignment  is  an  optimal  policy.  The  task  set 
is  not  schcdulable  using  any  static  priority  assignment  method  if  it  is  not  schcdulable 
using  the  rate  monotonic  method  as  explained  in  [9],  [10]. 

The  proposed  scheduling  algorithm  can  now  be  expressed  in  the  following  proposition. 

Proposition  1.  Consider  the  problem  of  controlling  n  decoupled  uncertain  nonlinear 
systems  using  the  Rl  lC  scheme.  The  following  equations  serve  as  the  nominal  model  for 
describing  the  decoupled  subsystems 

AT,  =  f^{x^{t),U^{t)) 

(11) 
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The  actual  subsystem  arc  described  by 


yn  =  fn{yniO,H„it))+S„{t,y„,U„) 


(12) 


In  equation  (12),  gi(t,y,u),  i=1.2 . n  accounts  for  disturbances  and  modeling  uncertainty 

and  its  norm  is  upper  bounded  by  h,.  The  function  f  in  equations  (11)  and  (12)  is 
Lipschitz  continuous  in  the  domain  of  operation  of  the  system  with  a  Lipschitz  constant 
represented  by  Li. 

It  is  useful  to  note  that  the  application  of  proposed  scheduling  method  to  multiple  UAVs, 
is  to  control  some  UAVs  with  limited  computational  resource;  for  example  controlling  2 
UAVs  using  only  one  computer.  In  this  case  the  nominal  model  used  in  controller  (to 
predict  the  states  and  inputs  over  the  prediction  horizon)  can  be  considered  as 

X,  =f|(X|(0,U,(/)) 

X,  =U(x,(/),u,(0) 


while  the  actual  UAVs  have  different  models.  The  source  of  this  difference  between  two 
models  can  be,  for  example,  because  of  the  friction  or  the  other  model  uncertainty  which 
may  exist.  Here  the  actual  model  is  represented  by 

y,  =f,(y,(0,u,(0)+g|(/,y,,u,) 

(14) 

y2  =f2(y2(0,l'2(0)+g2(^y2’“2) 

There  are  two  differences  between  the  actual  and  nominal  models: 

1-  The  state  variables  arc  different,  because  in  nominal  model  they  arc  predicted 
based  on  the  nominal  model  and  in  reality  they  are  affected  by  the  real  system. 
Here  x^  represent  the  predicted  states  from  nominal  model  and  y,  represent  the 
actual  states. 

2-  The  other  difference  is  the  difference  between  two  models  because  of  the 
uncertainty  which  discussed  before,  represents  the  foresaid  difference. 

If  there  is  only  one  computing  resource  available  for  this  set  of  n  subsystems,  the 
execution  horizon  for  each  subsystem  should  be  chosen  so  that  the  set  of  tasks  remain 
schedulablc.  This  should  be  carried  out  considering  the  fact  that  the  level  of  uncertainty 
in  each  subsystem  is  different. 

The  following  scheduling  approach  can  be  used  to  minimize  the  state  prediction  error  of 
the  uncertain  subsystems.  If  the  rate  monotonic  priority  assignment  method  is  used,  the 
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solution  to  the  following  optimization  problem  minimizes  the  overall  upper  bound  of  the 
state  predietion  error  on  all  subsystems,  while  the  set  of  tasks  remain  schedulable. 


linimize  -1) 

(=1  4 


s.t.  < 


X^</7(2"-1) 

o<s,<  4.,., 


(15) 


where  or,  and  S,.,,  represent  the  weighting  parameter  and  computation  time  for  task  / 
respectively.  Suh.i  is  the  upper  bound  for  the  execution  horizon  which  is  usually  some 
fraction  of  the  prediction  horizon.  L,  is  the  Lipschitz  constant  of  the  function  /.  In  the 
proposed  optimization  problem  the  optimization  parameters  arc  the  execution  horizons 
for  each  subsystem,  Si, 


Proof.  As  described  in  Section  2.2.1  {Retarded  Actuation  method,  type  ii),  the  RHC 
implementation  scheme  involves  prediction  of  the  states  at  the  next  sampling  time  and 
using  them  as  initial  conditions  to  solve  the  open-loop  optimal  control  problem  in 
advance.  The  prediction  error  for  subsystem  /  is  described  by  ||>',(^  , +4)~  4 1  • 

The  right  hand  side  of  the  following  inequality,  which  is  a  direct  consequence  of  the 
Bcllman-Grownwall  Lemma,  is  an  upper  bound  on  the  prediction  error  for  each 
subsystem  [13],[14] 


K'*,,  +  -l)  /  =  1,2,. ..,n 


(16) 


where  4,,  is  the  time  at  which  sampling  of  the  states  for  subsystem  i  has  taken  place. 
Evidently  •*'(/*,)=>’(/*,)  for  i=J.2 . n  since  4,,  is  the  time  at  which  the  sampling  has 

taken  place  and  there  is  no  mismatch  between  the  states  of  the  nominal  and  actual  system. 
Therefore  ( 1 6)  reduces  to 


The  right  hand  side  of  equation  (1 7)  justifies  the  chosen  fomi  of  the  objective  function  in 
(15).  Furthermore,  for  a  series  of  task  to  be  schedulable  equation  (10)  should  hold.  This 
together  with  an  upper  bound  on  the  desired  execution  horizon  for  each  subsystem 
constitutes  the  constraints  of  the  optimization  problem.  □ 

Remark  2.  In  [15]  the  author  discusses  the  fact  that  a  larger  error  in  the  prediction  of  the 
initial  conditions  results  in  performance  degradation  and  possibly  instability.  Therefore 
the  mismatch  should  be  minimized.  If  it  is  assumed  that  a  larger  prediction  error  results  in 
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worse  performance  of  the  system  then  the  objective  function  in  equation  (15)  maximizes 
the  overall  performance  of  the  system. 

Remark  3.  In  Proposition  1,  the  weighting  parameter  a,  is  a  design  parameter  by  which 
the  prediction  error  in  each  subsystem  can  be  controlled.  A  higher  weight  is  equivalent  to 
a  lower  desired  prediction  error  for  that  subsystem. 


5. 2. 2.3  Discussion 


The  optimization  problem  discussed  in  Proposition  1  is  based  on  the  Bellman-Grown 
Lemma.  However,  due  to  the  conservative  nature  of  this  lemma  in  finding  an  upper 
bound  on  the  state  prediction  error,  the  increase  in  robust  performance  may  not  be  fully 
optimal.  Use  of  more  accurate  less  conservative  approaches  for  bounding  the  state 
prediction  error,  such  as  differential  inclusions  [20],  can  potentially  improve  the  method. 
The  proposed  approach,  however,  has  a  clear  advantage  over  more  sophisticated 
computationally  expensive  bounds.  The  cost  function  in  (15)  for  Proposition  1  should  be 
modified  accordingly,  depending  on  the  specific  method  used  for  finding  the  state 
prediction  error  bound. 


5.2.3  Example 


In  this  section,  the  proposed  method  described  in  Section  5. 2. 2. 2  is  applied  to  RHC 
stabilization  of  two  double-integrator  systems  using  a  single  computer.  The  dynamic 
equations  describing  a  differentially  driven  wheeled  mobile  robot,  or  a  rotorcraft-like 
UAV  Hying  at  constant  altitude,  can  be  transformed  into  two  double  integrators  using 
feedback  linearization  [16],  [17],  [18].  Therefore,  controlling  a  set  of  double  integrators  is 
potentially  useful  for  autonomous  formation  guidance  of  multiple  unmanned  systems, 
where  it  is  desired  that  each  agent  takes  a  predefined  position  [19]. 

The  nominal  system  is  described  by  the  set  of  equations 


i-T.2  =  '6 

where  .v,  /,  .vy.j  represent  the  states  and  represents  the  input  to  the  /th  system.  In  the 
simulations,  it  is  assumed  that  the  actual  system  is  described  by  the  following  equations 
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=  -^1,2 

li,  2  =  tr,  -i-0.5sin(/.r| ,) 

|-^2.,  -  -’^2,2 

[.V2  2  =  1*1  +  0-25  sin(r.r2 1 ) 


(19) 


T  7  ' 

with  initial  conditions  chosen  to  be  .V(;=[9  -4]  for  the  first  subsystem  and  .Vo=[4  1  ]  tor  the 
second  subsystem.  Among  the  available  RHC  methods,  the  quasi-infinite  RHC  approach 
described  in  [1 1]  is  implemented.  The  prediction  horizon  is  selected  as  5  seconds  and  the 
weighting  matrices  Q  and  R  are  taken  as  identity  matrices  for  both  subsystems.  The 
matrix  P  in  (4)  is  found  by  solving  the  Lyapunov  equation  (9)  in  reference  [11]. 

If  the  2-norm  is  used,  the  parameters  found  in  the  optimization  problem  discussed  in 
equation  (15)  will  take  the  following  values:  /;/=2.23,  h:=0.5  and  Li-  £7=1.  The 
weighting  parameters,  «„  and  the  upper  bound  on  the  execution  horizon  are  selected  as  1 
and  0.25  respectively,  for  both  subsystems.  It  is  assumed  that  a  zero  control  signal  is 
applied  to  the  system  before  the  onset  of  RlIC  initialization.  The  optimization  problem 
described  in  Proposition  1  was  solved  using  MATLAB  and  the  execution  horizons  were 
found  to  be  (5'/=0.7965  and  <^>7=0.8996.  Therefore,  using  a  rate  monotonic  approach,  the 
highest  priority  is  assigned  to  the  first  subsystem.  The  controllers  for  the  first  and  second 
subsystems  start  the  sampling  at  times  of  c=0.9465  and  r-0.8996  respectively.  This 
implies  that  before  this  time  the  controllers  were  in  the  off  mode  and  no  input  was 
applied  to  the  system.  To  investigate  the  pre-emption  of  the  second  subsystem  by  the  first 
one,  a  0.15  second  delay  for  the  start  time  of  the  first  subsystem  controller  was  applied 
(see  Figure  5).  Numerical  simulations  indicated  that  ■  <  0.35  . 

Subsystem  I 
Subsystem  2 

0.9465  1.743  2.5395  3.336 


5,= a  7965 

> 

0.8996 

1.7992  2.6988 

3.5984 

,4 

6j=0.8996 

»  i 

1 

1 

- 

,1 

1 

1 

1 

J _ 

time  (s) 


Figure  5-  Diagram  showing  the  timing  for  eaeh  individual  subsystem 

To  solve  the  optimal  control  problem,  the  states  and  inputs  were  approximated  by  a  set  of 
piecewise  polynomials  of  third  order  with  second  order  continuity  at  the  boundaries, 
which  is  an  adaptation  of  the  scheme  described  in  [5].  In  order  to  solve  the  resulting 
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optimization  problem,  the  NAG  C  Library  (Mark  7)  was  used  on  a  2.4  GHz  Pentium  4 
computer.  The  Venturcom  RTX  (version  5.1)  real-time  operating  system  was  used  for 
implementation.  Numerical  simulation  was  carried  out  using  the  C  programming 
language. 

Two  main  functions  used  from  NAG  are  “e04ucc”  [36]  and  “e04xzc”  [37]'’. 

The  response  of  the  closed-loop  RHC  system  for  the  first  and  second  subsystems  is 
shown  in  Figure  6  and  Figure  7  respectively.  It  is  apparent  that  the  states  of  both 
subsystems  are  stabilized  around  the  origin  despite  the  perturbations  introduced  to  the 
system.  It  is  also  evident  that  the  proposed  scheduling  approach  achieves  similar  levels  of 
performance  for  each  subsystem  even  though  subsystem  1  has  significantly  higher 
uncertainty  than  subsystem  2.  This  is  a  consequence  of  the  higher  priority  allocated  to 
subsystem  1  which  improves  its  performance  by  reducing  the  computational  delay 
compared  to  subsystem  2.  The  scheduling  of  the  CPU  time  is  shown  in  Figure  8.  As 
shown  in  Figure  8,  subsystem  2  starts  the  optimization  process  at  /==0.8996  and  is  pre¬ 
empted  by  subsystem  1  at  /=0.9465  due  to  the  fact  that  subsystem  1  has  a  higher  priority. 
Subsystem  2  is  remained  pre-empted  until  1.28 18  and  at  this  time  the  computation  of 
subsystem  2  is  started  (from  finishing  computation  of  first  subsystem  until  beginning 
computation  of  the  second  subsystem,  the  pre-emption  overhead  should  be  considered  in 
timing).  The  computation  of  subsystem  2  is  finished  on  /=  1.5298.  Therefore  the 
computing  time  for  subsystem  1  (with  pre-emption  overhead)  is  0.3353  seconds  and  the 
total  computing  time  for  subsystem  2  (with  pre-emption  overhead)  is  0.2948  seconds. 
The  pre-emption  process  is  repeated  again  between  6  and  8  seconds. 
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0 


/  . 

2  4  6 

time  (s) 


X 
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8  10  12 


Figure  6-  Time  history  of  the  states  for  the  first  subsystem 


■'  Since  this  work  (with  NAG)  is  going  to  be  redone  using  RIICOOL  and  SNOPT  [38],  no  more  explanation 
is  done  in  this  report. 
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Figure  7-  Time  history  of  the  states  for  the  second  subsystem 
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Figure  8-  CPU  time  schedule  (I=running,  0.5=Pre-empted,  0=ldle)  for  the  first  subsystem  (top) 

and  the  second  subsystem  (bottom) 


5.2.4  Discussion 
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In  this  section,  a  systematic  approach  is  developed  for  scheduling  computation  and 
determination  of  the  execution  horizon  for  multiple  uncertain  RHC  systems.  It  was 
shown  that  using  a  rate  monotonic  priority  assignment  method  combined  with  analytical 
bounds  on  the  prediction  error,  the  problem  of  scheduling  multiple  uncertain  plants  can 
be  cast  into  an  appropriate  constrained  optimization  problem.  The  constraints  guarantee 
that  the  processes  will  be  schedulable,  and  the  optimization  provides  performance 
robustness  to  uncertainty.  The  proposed  method  was  applied  to  a  double  integrator 
example  problem  demonstrating  the  validity  of  the  approach. 

In  the  next  sections  the  proposed  algorithm  will  be  applied  to  a  set  of  under  actuated 
3DOF  miniature  hovercraft.  Experimental  data  ensures  the  performance  of  the  proposed 
approach. 
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5.3  Application  to  3DOF  Miniature  Hovercraft  Problem 

5.3.1  Modeling 


The  aim  of  this  section  and  the  next  one  is  to  identify  the  parameters  of  Miniature 
Hovercraft.  Since  RHC  approach  is  Model  Based,  Finding  these  parameters  are  critical 
for  implementing  the  RHC  approach  to  the  miniature  hovercraft. 

For  obtaining  the  equations  of  motion  of  the  miniature  under  actuated  hovercraft,  three 
coordinate  frames  are  defined  (Figure  9). 

{G}  is  the  Global  coordinate  frame  (in  Figure  9,  the  black  axes  X  and  Y  denote  for 
the  Global  coordinate  frame.) 

{B}  is  a  rotating  coordinate  frame  which  the  center  of  this  frame  is  the  center  of  {G} 
and  the  X g  axis  is  always  in  the  same  direction  with  the  symmetric  axis  of 
hovercraft,  (in  Figure  9,  the  blue  axes  X g  and  Yg  are  illustrating  the  coordinate 
frame  {B}) 

{M}  is  a  moving  coordinate  which  is  attached  to  the  hovercraft.  This  is  defined  in 
order  to  make  the  definition  of  {B}  easier.  The  axis  of  |B}  and  {M}  are  always  in 
the  same  direction  but  the  center  of  these  two  coordinate  frames  are  different,  (in 
Figure  9,  the  red  axes  X'g  and  Yg  are  illustrating  {M}). 

The  variables  which  are  used  in  this  part  are  as  follows^: 

M  The  total  mass  of  hovercraft 
J  The  moment  of  inertia  of  hovercraft  around  Z  axis 

The  coefficient  of  viscous  friction  in  the  forward  movement 

^2  The  coefficient  of  viscous  friction  in  the  side  movement 

by  The  coefficient  of  viscous  friction  in  rotation 

Force  caused  by  left  side  motor 
Fy  Force  caused  by  right  side  motor 
u  Velocity  of  hovercraft  along  Xg  axis 

V  Velocity  of  hovercraft  along  Yg  axis 

r  Angular  velocity  of  hovercraft  around  Z  axis 
j7  Total  velocity  vector  (Velocity  in  {G}) 

g  Total  Acceleration  Vector  (Acceleration  in  {G}) 


’  As  the  variables  are  only  used  in  this  section  and  do  not  affect  the  whole  report,  they  are  not  mentioned  in 
the  Nomenclatures  in  the  beginning  of  the  report. 
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/  Distance  between  the  centers  of  two  propellers 


F2/ 

■/ 


/F1 


Figure  9-  a  schematic  drawing  of  hovercraft  and  coordinate  systems 
The  velocity  of  hovercraft  in  the  Global  coordinate  frame  (G}  can  be  defined  as  follows: 
V  =  ul^+vj,  (20) 

The  acceleration  can  be  defined  by  derivation  of  velocity  vector  as  follows: 


o„.a  =  ^ 

V  =Uig+ulB+Vjg+Vjg 

(21) 

Since  the  {B}  coordinate  is  rotating  with  the  rate  of  r 
used  [26]: 

,  the  following  equations  can  be 

's  =r]g 

1b  =  ^^Jb  =-''4 

(22) 

Therefore  the  following  equation  can  be  derived  from  equations  (20),  (2 1 ),  and  (22) 

«//.B  =  “  4  +  Jb  +  ^1b-  4  =  (w  -  4  +  (v  +  iir)  1b 

(23) 
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If  no  friction  exists,  from  Newton’s  Second  Law; 

M(m  -  vr)  =  F|  +  Fj 
Miv  +  ur)-0 
Jr=y^{F,-F,) 

If  viscous  friction  is  considered  we  have  the  following  equations: 

M{ii  -V  r)  =  F^  ■¥  F^  -  u 
M{v  +  u  r)  =  -/t,  V 

Jr=y{F,-F,)-b,r 

where  the  b^,  A, ,  and  b^  are  the  coefficients  of  viscous  friction  in  ,  Y^,  and  rotation 
directions  respectively. 

Equations  (25),  (26),  and  (27)  are  the  same  as  equations  (2a),  (2b),  and  (2c)  in  [22] 
respectively,  except  that  in  [22]  the  coefficients  of  viscous  friction  are  considered  to  be 
the  same  for  all  directions. 

As  mentioned  above,  the  aim  of  this  section  is  to  use  the  identified  model  in  the  RHC 
approach  for  performing  the  experimental  verification.  So  the  equations  (25),  (26),  and 
(27)  should  be  rearranged  by  performing  some  algebraic  operations  in  order  to: 

•  reduce  the  number  of  unknowns 

•  make  the  input  signal  (output  of  controller)  compatible  with  the  hardware 

The  first  part  is  for  simplicity  in  application  of  model  identification  and  the  second  part  is 
the  main  reason  of  model  rearranging.  In  reality  the  controller  output  will  be  applied  to 
the  applied  voltage  of  the  motors  of  hovercraft.  So  it  makes  sense  to  use  the  applied 
voltages  directly  as  the  inputs.  If  the  relation  between  the  produced  forces  by  propellers 
and  the  applied  voltage  to  the  motors  can  be  considered  linear^,  the  reformed  equations  of 
motion  will  be  as  follows: 

u -vr  -  b\u  +  +  021(2  (28) 

v  +  t<r  =  b'yV  (29) 

r  =  b'^r  +  +  0^1(2  (20) 


(25) 

(26) 
(27) 


*  A  better  estimation  for  relation  beKveen  the  applied  voltage  and  the  produced  forces  by  propeller,  have 
been  discovered,  and  best  results  were  obtained  when  produced  force  was  related  to  the  linear  composition 
of  voltage  and  square  of  voltage, 
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In  the  equations  (28)  to  (30),  tq  and  Uj  are  the  normalized  input  voltages  to  the  left  and 
right  motors  and  vary  from  -1  to  1.  b[ ,  b'^,  and  b^  are  new  forms  of  viscous  friction 


coefficients  and  equal  to 


'M 


respectively. 


5.3.2  System  Identification 

In  this  section  the  unknown  parameters  of  the  model  described  in  equations  (28),  (29), 
and  (30)  will  be  defined.  The  parameters  that  must  be  defined  are  b[  ,b[  ,b[,a^ ,c/, , 

and  . 

In  order  to  perfomi  identification,  the  data  from  position  and  orientation  of  hovercraft  are 
needed.  The  sensor  that  used  for  that  purpose  is  PClBird’  [23]  which  has  the  capability  of 
measuring  Position  and  Orientation  in  3D  space  with  respect  to  a  Global  Co-ordinate 
frame.  Therefore  the  data  from  PCIBird  need  two  modifications: 

•  Coordinate  transformation 

•  Obtaining  velocities  and  acceleration 


Coordinate  transformation 

Since  the  equations  of  motion  are  expressed  in  ]B}  the  coordinate  transformation  is 
necessary.  It  should  be  noted  that  the  equations  dealing  with  velocities  can  be 
transformed  using  a  rotation  matrix.  Equations  (31)  and  (32)  describe  the  results. 

i/  =  2fcos(i/'-i-Tsin^i/  (31) 

v  =  -2fsinti/-(- Tcost//  (32) 

But  ii  and  v  can  not  be  calculated  using  a  rotation  matrix.  They  must  be  derived  by 
direct  derivative  of  u  and  v,  respectively  because  of  their  physical  meanings.  The  results 
are  as  follows  by  derivation  of  equations  (31)  and  (32); 

ti  =  X cost//  -2f/-sin [j/  +  Y smif/  +  Yreo^y/  (33) 

V  =  -X sin^  -  Xrcosy/  -i-  Y cosy/  -  Trsin  y/  (34) 


Obtaining  velocities  and  acceleration 


’  The  identification  can  also  be  done  using  the  combination  of  Vision  system  and  InertiaCube.  These 
sensors  will  be  explained  in  section  4.  The  main  reason  of  using  PCIBird  is  that  the  data  measured  by  this 
sensor  is  more  accurate  than  vision  system  and  also  all  of  the  data  can  be  measured  by  one  sensor. 
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As  briefly  mentioned,  the  outputs  of  PClBird  [23]  are  only  position  and  orientation, 
therefore  velocity  and  acceleration  should  be  calculated  from  the  original  position  and 
orientation  data.  Since  the  hovercraft  model  is  2DOF  only  X,  Y,  and  Yaw  will  be  used. 
Velocity  and  acceleration  should  be  calculated  using  the  following  “Finite  Difference 
Method”: 


X 


n 


^n.l  -  -y. 

^)+l 


(35) 


where  and  denote  for  position  (X  axis)  and  time  of  the  n'''  sampling  data.  The 
same  equation  is  used  for  other  directions. 

As  a  result  of  that,  the  noises  in  measurement  can  cause  significant  errors  in  calculated 
velocities  and  accelerations.  To  avoid  the  noise  effect,  the  differentiation  in  equation  (35) 
should  be  done  in  a  larger  time-step.  So  a  revised  form  of  equation  (35)  can  be 
considered  as  follows: 

^  filler  ~  ^ n 

A'„=— ^ — 17“ 

filler  ‘n 

By  selecting  a  proper  filter  value,  the  noise  effect  can  be  reduced  and  neglected.  The 
following  two  steps  are  used  in  order  to  calculate  ii  ,v  ,r  ,ii  ,v ,  and  r  : 

Step  1 

From  equation  (36)  the  X ,  Y ,  and  r  values  are  calculated  from  the  measured  data  (X, 
Y,  and  Yaw  (y/)).  At  this  slep  filter  I  is  used  as  ihs  filter  value.  By  using  this  data 
filtering,  all  of  the  data  which  are  used  in  model  identification  must  be  updated. 
Therefore  a  new  set  of  data  will  be  produced  in  this  step.  Therefore  parameters  t  and  y/ 
will  be  updated  using  the  following  equation: 

^^^n.new  ~  ri  +  filler  )  (37) 

where  Var  denotes  for  the  parameter  that  have  to  be  updated. 

Step  2 

From  equation  (36)  the  X ,  Y ,  and  r  are  calculated  from  the  calculated  data  in  the  first 
step.  At  this  sXcp  fller2  is  used  as  \he  filter  value.  Same  as  step  I,  another  set  of  data  must 
be  calculated.  Therefore  X  ,Y  ,r ,  t  ,and  y/  will  be  calculated  from  their  values  in  the 
Step  I,  using  equation  (37). 

Step  3 

From  equations  (3 1 )  to  (34)  iY,v,j/,andv  will  be  calculated. 
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5.3.2. 1  Experimental  results 

In  the  following,  the  results  of  some  different  tests  have  been  shown.  In  all  of  the 
following  results  the  filter  values  were  100  and  70,  respectively  unless  other  specified. 


5.3. 2. 1.1  Using  input  signal  -  Forward  movement 

In  the  following  set  of  data,  the  same  command  is  sent  to  the  motors  (i/i  -1(2)  so  the 
movement  was  mainly  in  the  X g  {X  axis  of  {B}).  Consequently,  it  is  better  to  use  these 
results  only  for  equation  (28). 

In  this  part  the  results  of  four  data  sets  have  been  presented. 

Dalai: 

In  this  case  the  input  signal  was  0.5  ( u^  -  Uj  =  0.5 )  and  the  results  were  as  follows; 
b^'  =-0.29 

ai  =  (72  =  0. 1 1 

The  results  are  shown  in  Figure  (10). 


Figure  10-  u  {tn/s),  ii  original  fined  )  3re  shown  for  Dalai  of  Pari  one 

Data4: 

In  this  case  the  input  signal  was  0.7  ( (/|  =  U2  =  0.7  )  and  the  results  were  as  follows: 
h[  =  -0.25 
a,  =  aj  =0.13 

The  results  are  shown  in  Figure  (11).  This  result  was  not  correct  and  the  friction 
coefficient  was  more  than  zero  which  is  not  reasonable. 
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U 
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Figure  11-  ti{m/s),  are  shown  for  of  Prr/T  owe 

Data?: 

In  this  case  the  input  signal  was  0.85  (r/j  =  Uj  -  0.85 )  and  the  results  were  as  follows: 
6,'  =-0.14 

Oi  =  <32  =0.12 

The  results  are  shown  in  Figure  (12). 


original  i  i 
'  Fitted  ! 


1.8  2  2.2 

Figure  12-  uimjs),  are  shown  for  D<:r/o7  of  Po/V  (wre 

Data9: 
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In  this  case  the  input  signal  was  1 .0  ( u,  =112  =1.0)  and  the  results  were  as  follows: 

=  -0.07 

CKl  =  O2  =  0. 10 

The  results  are  shown  in  Figure  (13). 

0.3 
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O.I5I 
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S'a  1  1.2  1.4  16  18  2  2.2 

Time  (s) 

Figure  13-  u{m/s),  iig^jgj^^ij{mls^),an(J  11  arc  shown  (or  Data9  oi' Part  one 

5.3. 2. 1.2  Using  input  signal  -  Rotation 

In  this  part  the  hovercraft  was  oscillating  around  zero  angle.  As  a  result,  this  data  should 
be  used  only  for  equation  (30).  This  oscillation  was  done  by  using  a  P  controller  (P=0.5) 
and  only  one  set  of  data  is  shown, 

Data2: 

The  results  were  as  follows: 
hn,'  =  -0.05 

aj  =-1.2 
04  =1.2 

The  results  are  shown  in  Figure  (14). 
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Figure  14-  y/{rad),  i//{rad/s),  ipg^ig^„^,(radls^),and ij/  fj,i^.j{radls')  are  shown  for  Data 2  of 

Part  two 


5.3. 2. 1.3  No  input  signal 

In  this  part,  no  signal  applied  to  the  motors  and  initial  velocity  was  used,  instead.  The 
result  of  this  test  was  used  to  identify  the  friction  coefficients  in  the  equations  (28)  and 
(29), 

a)  Calculating  h\  from  equation  (28) 

It  was  varying  between  -0.13  and  -0.56.  Some  of  the  graphs  are  shown  here: 

Data5: 

The  results  were  as  follows: 

6,’  =  -0.25 

The  results  are  shown  in  Figure  (15). 
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Figure  15-  w {nils^),and  (w/.r)  arc  shown  for  DalaS  of  Part  three 


DataS: 

The  results  were  as  follows: 

h[  ^-Q.\5 

The  results  are  shown  in  Figure  ( 1 6). 
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Figure  16-  i<  i'ti/s~),and  it  (m/s~)  arc  shown  iov  DataH  oi Part  three 


b)  Calculating  b\  from  equation  (29) 

It  was  varying  between  -0. 12  and  -0.02.  One  of  the  graphs  is  shown  here: 
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Data2: 

=  -0.02 

The  results  are  shown  in  Figure  (17). 
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Figure  17-  v (w/.v" )  are  shown  \'ox  Dalci2  of  Part  three 


5.3.2.2  Discussion 

From  the  presented  identification  algorithm,  the  following  values  have  been  defined. 
Some  of  the  results  are  in  the  report. 


b[  ; 

-0.56,  -0.30,  -0.29,  -0.25,  -0.25,  -0.24,  -0.18,  -0.15,  -0.14,  -0.13,  -0.07,  0.03,  0.03, 
and  0.05.  By  reducing  -0.56  and  the  positive  values  and  averaging  between  the 
remaining  data,  A,' =  -0.20 

-0.12,  -0.02.  By  averaging  we  have:  =-0.07 

b'y- 

-0.18,  -0.14,  -0.05,  0.02,  0.08,  0.10,  0.13,  0.24,  0.24.  By  reducing  the  positive 
values  and  making  average  between  the  remaining  data  we  have:  b'^  =  -0. 12 

a,\ 

0.07,  0.08,  0.10,  0.10,  0.11,  0.11,  0.12,  0.13,  and  0.14.  By  averaging  we  have: 

C7|  =0.1 1 

Oj-. 

same  as  o, .  So  a,  =  0.1 1 
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o, ; 

-0.10,  -1.02,  -1.08,  -1.13,  -1.20.  By  making  average  we  have:  r/,  =-1.1 

a,-. 

same  as  -Oj.  So  a^  =1.1 

5.3.3  RHC  Object  Oriented  Library 

In  order  to  implement  RHC  to  an  experimental  apparatus,  you  have  to  do  different  things 
like,  Optimization  and  spline  interpolation.  In  addition,  to  apply  it  to  different  apparatus 
and  schedule  them  in  one  computer,  it  has  to  be  object  oriented.  The  RHC  Object 
Oriented  Library  (RHCOOL)  is  developed  in  C++  and  greatly  simplifies  the 
implementation  of  scheduled  RHC  hard  real-time  simulations  and  experimental 
implementations  for  multiple  UAV  systems.  For  implementing  RHC  to  different 
apparatus  by  using  the  RHCOOL,  only  a  few  programming  is  required  which  are  mainly 
for  definition  of  model  and  constraints. 

Following,  first  the  library  will  be  described  and  all  of  the  parameters  and  functions 
which  have  been  defined  for  implementation  will  be  illustrated.  After  being  familiar  with 
the  library,  an  example  of  3DOF  helicopter  will  be  solved  and  described. 

5.3.3. 1  Description  of  the  Library 

The  aim  of  this  part  is  to  describe  the  main  functions  and  parameters  of  RHCOOL  to  a 
reader  who  wants  to  work  with  the  library.  We  assumed  that  the  reader  is  familiar  with 
the  basic  specifications  of  RHC.  In  addition  the  user  should  be  familiar  with  the  basics  of 
programming  with  C++. 

There  are  six  files  in  the  library  that  the  user  should  be  familiar  with.  These  files  are 
program,  epp  (program  _heli_local.  epp),  RHC_user.  epp  (RHC _user Jiel  ijocal.  epp) , 
RHC.h,  dVector.cpp,  dMatrix.cpp,  and  dMatri.x_array.cpp  which  will  be  described  in  the 
following  sections. 

5.3.3.1.1  RHC.h 

This  file  defines  a  class  with  name  of  RHC  which  all  of  the  parameters  and  functions  of 
the  library  are  defined  in  it.  RHC.h  is  available  in  the  Appendix  B.  The  public  parameters 
of  RHC  class  are  as  follows: 

to 

tO  is  a  double  variable  and  denotes  the  initial  time.  It  is  the  time  that  controller  is  started 
and  usually  is  equal  to  zero.  It  does  not  change  during  the  optimization. 

Remark:  Figure  (18)  illustrates  the  different  “times”  used  in  the  library. 
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Actual  trajectory 
predicted  trajectory 
Spline  Interpolation  Point  • 
Spline  Control  Point  X 


T/(Ns-1) 
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Control  Point  No 
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'k 


«X  • 


•  •  . 


.  1  o 

X  2 


,  Ni 
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tb  t 


ti(k-1]+tb 


ts[m-1]+tb 


tb+T  Time 


Figure  18-  different  “times”  used  in  RHCOOL 


t 

t  is  a  double  variable  and  denotes  the  current  time. 

tb 

tb  is  a  double  variable  and  denotes  the  time  that  interpolation  begins.  The  RMC  optimizes 
the  outputs  for  the  prediction  horizon  (T)  and  applies  the  result  to  the  execution  horizon 
(delta).  Therefore  each  optimization  will  start  at  time  tb,  and  end  at  time  tb+T. 

Remark: 

It  is  useful  to  describe  the  library  more.  Figure  19  illustrates  two  systems,  one  is  the  input 
calculated  by  controller  and  the  other  is  the  input  applied  to  Actual  system.  At  time  tb, 
the  sampled  data  are  ready  and  controller  starts  to  calculate  the  input  over  the  period  of  tb 
to  tb+T.  This  calculation  finished  at  time  tb  +  5^^  and  the  calculated  inputs  are  starting  to 

be  applied  to  the  system  at  time  tb  +  Tp.  So  the  inputs  applied  to  the  system  with  a  delay 

equal  to  the  execution  horizon  ( 5  which  is  equal  to  the  period  ( 7^  )  of  the  periodic  task 

defined  for  the  controller).  One  solution  for  this  delay  is  using  Retarded  Actuation 
Technique,  which  is  mentioned  in  section  2.  In  RllCOOL  retarded  actuation  does  not 
applied  at  this  step  for  simplicity.  So  the  calculated  input  in  the  period  of  tb  to  tb  +  T  is 
should  be  applied  to  the  system  in  the  period  of  tb+T  to  tb+2T  (the  red  lines  in  Figure  19 
are  the  same).  Another  issue  related  to  the  RFICOOL  is  that  the  calculated  inputs  are 
applied  to  the  system  using  First  Order  Hold  approximation.  So  during  the  execution 
horizon,  the  input  values  do  not  change.  It  is  not  a  big  issue  and  is  supposed  only  for 
simplicity  and  will  be  fixed  in  the  next  version  of  the  library. 


“  is  the  computational  time. 
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6=Tp 


A 


3 

Q. 
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tb 


delay 


Controller 


Time 


[  Actual  system 

1 

I 

tb  tb+Tp  tb+2Tp  Time 

Figure  19-  Illustration  of  tb,  execution  horizon  =  T^,),  and  computation  time  ( 5^  ). 


*ti 

*ti  is  a  dVector  pointer  and  denotes  the  interpolation  time.  When  the  interpolation  is 
going  on,  the  time  at  break  points  (interpolation  points)  of  the  spline*^  form  the  ti  vector 
and  each  element  of  this  vector  refers  to  the  time  of  each  interpolation  point.  The 
elements  of  ti  changing  from  0  to  {Ni-\)dti=T. 


*ts 

*ts  is  a  dVector  pointer  and  denotes  the  time  at  spline  points.  In  addition  to  the  statement 
about  ti,  the  time  in  the  control  points  of  spline  fonn  the  ts  vector.  The  components  of  ts 
varying  from  0  to  T. 

dti 

dti  is  a  double  variable  and  denotes  the  interpolation  time  interval.  It  is  equal  to  T/(Ni-l). 

T 

7  is  a  double  variable  and  denotes  the  RlIC  prediction  horizon.  During  this  time,  the 
optimization  is  done. 

delta  (5) 

delta  is  a  double  variable  and  denotes  the  RHC  execution  horizon.  The  execution  horizon 
is  the  time  that  calculated  inputs  from  optimization  are  applied  to  the  system. 

Nx 

Nx  is  an  int  variable  and  denotes  the  number  of  state  variables. 


**  See  [29]  for  infomiation  about  cubic  splines.  This  document  is  available  in  the  CD  attached  to  the  report 
(c3-3.pdf). 
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Nu 

Nil  is  an  int  variable  and  denotes  the  number  of  inputs. 

Nz 

Nz  is  an  int  variable  and  denotes  the  number  of  flat  outputs.  See  [5]  for  more  information 
about  flat  outputs. 

Ny 

Ny  is  an  int  variable  and  denotes  the  number  of  outputs. 

*X0 

*X0  is  a  dVector  pointer  and  denotes  the  initial  condition  of  state  variables. 

*U0 

*U0  is  a  dVector  pointer  and  denotes  the  initial  inputs. 

*X 

*X  is  a  dVector  pointer  and  denotes  the  current  state  (at  time  t).  The  number  of 
components  of  this  vector  is  equal  to  Nx. 

*U 

*U  is  a  dVector  pointer  and  denotes  the  current  input.  The  number  of  components  of  this 
vector  is  equal  to  Nu. 

*Y 

*Y  is  a  dVector  pointer  and  denotes  the  current  output.  The  number  of  components  of  this 
vector  is  equal  to  Ny. 

*Z 

*Z  is  a  dVector  pointer  and  denotes  the  current  flat  output.  The  number  of  components  of 
this  vector  is  equal  to  Nz. 

*Xd 

*Xd  is  a  dVector  pointer  and  denotes  the  current  state  derivatives. 

Ns 

Ns  is  an  int  variable  and  denotes  the  number  of  spline  control  points  for  each  output. 
Number  of  control  points  is  similar  to  the  degree  of  polynomial.  As  discussed  later,  the 
varying  control  points  are  the  parameters  which  will  be  find  by  optimization. 

Ni 

Ni  is  an  int  variable  and  denotes  the  number  of  spline  interpolation  points  for  each  output. 
After  finding  the  parameters  of  spline  by  optimization,  the  values  of  that  spline  will  be 
recorded  in  the  interpolation  point.  Therefore  the  interpolation  points  (sometimes  called 
spline  break  points)  change  the  continuous  spline  curve  to  discrete  values  (make  a 
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tabulated  table).  Ni  should  be  large  enough  to  make  interpolation  points  close  enough  to 
each  other. 

Nc 

Nc  is  an  int  variable  and  denotes  the  number  of  varying  control  points.  Az  mentioned 
above,  all  of  the  control  points  are  in  two  main  groups;  one  group  is  the  fixed  control 
points  because  of  the  constraint  and  the  other  group  forms  the  varying  control  points.  The 
second  group  will  be  defined  by  optimization, 

Nd 

Nd  is  an  int  variable  and  denotes  the  maximum  number  of  output  derivatives  required. 
For  calculating  state  variables  and  inputs  from  flat  outputs,  the  derivatives  of  the  flat 
outputs  are  required.  Nd,  is  the  maximum  derivative  required  from  all  of  the  flat  outputs. 

Ks 

Ks  is  an  int  variable  and  denotes  the  order  of  bspline  (must  be  at  least  Nd  +  1 ).  This  is 
used  when  bspline  used  for  interpolation. 

*Cs 

O'  is  a  dMatrix  pointer  and  denotes  the  spline  control  points.  All  of  the  spline  control 
points  form  the  matrix  Cs.  This  matrix  is  in  the  form  of  Cs[qJ[jJ.  q  is  the  related  flat 
output  and  j  is  the  related  control  point. 

*Cv 

Cv  is  a  dVector  pointer  and  denotes  the  vector  containing  all  varying  control  points.  All 
of  the  varying  control  points  from  all  of  the  flat  outputs  form  the  vector  Cv.  The 
definition  of  this  vector  (how  the  control  points  of  different  flat  outs  are  placed)  is 
defined  by  user  in  function  calc_J  the  RHCjnser.cpp  {RHC _user_heli JocaLcpp). 

*Xi 

Xi  is  a  dMatrix  pointer  and  denotes  the  state  variables  Xi[j]fi]  for  each  interpolation  point 
/.  As  mentioned  before,  all  of  the  values  of  state  variables,  inputs,  outputs,  and  flat 
outputs  are  recorded  in  the  interpolation  points.  So  the  matrix  Xi,  is  for  all  of  the  values 
of  state  variables  which  are  the  result  of  the  last  optimization  over  the  prediction  horizon. 

*Ui 

Ui  is  a  dMatrix  pointer  and  denotes  the  input  Ui[j][i]  for  each  interpolation  point  /.  The 
definition  of  Ui  is  similar  to  Xi,  and  represents  all  of  the  values  of  inputs  which  are  the 
result  of  the  last  optimization  over  the  prediction  horizon. 

*Yi 

Yi  is  a  dMatrix  pointer  and  denotes  the  output  Yi[j][i]  for  each  interpolation  point  i.  The 
definition  of  Yi  is  similar  to  Xi,  and  represents  all  of  the  values  of  output  variables  which 
are  the  result  of  the  last  optimization  over  the  prediction  horizon. 

*Zi 
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Zi  is  a  dMathx_array  pointer  and  denotes  all  of  the  flat  outputs  and  their  derivatives  at 
each  interpolation  time  {Zi[q][id][i]).  q  (1  to  Nz),  is  for  each  flat  outputs,  id  (0  to  Ncf)  is 
for  each  derivative  and  /  is  for  each  interpolation  time. 

*ZiO 

ZiO  is  a  dMatrix  pointer  and  denotes  flat  output  value  at  the  beginning  of  the  interpolation 
time  th  (Zi0[q] [id]).  ^  (1  to  Nz),  is  for  each  Oat  outputs  and  id  (0  to  Nd)  is  for  each 
derivative. 

*ZiT 

dMatrix  ZiT[q][id] 

Z/T  is  a  dMatrix  pointer  and  denotes  flat  output  values  at  the  end  of  the  interpolation  time 
tb+T  {ZiT[q] [id]),  r/  (1  to  Nz),  is  for  each  flat  outputs  and  id  (0  to  Nd)  is  for  each 
derivative. 

*Q,  *R,  and  *P 

These  are  cost  function  parameter  matrices  and  are  pointers  in  the  forni  of  dMatrix.  Q  is  a 
Nx  by  Nx  matrix,  R  is  a  Ny  by  Ny  matrix,  and  P  is  a  Nx  by  Nx  matrix. 

*Bs 

Bs  is  dMatrix_array  pointer  and  denotes  the  bspline  basis  functions.  Bs]id[[i][j]  for 
derivative  id  (0  to  Nd),  interpolation  time  i  (1  to  Ni),  and  control  point  j  (1  to  Ns).  This  is 
used  when  the  bspline  is  used  in  interpolation. 

*C2 

C2  is  a  dMatrix  pointer  and  denotes  the  second  derivative  at  control  points.  It  is  arranged 
in  the  form  of  C2[q][J]  for  flat  output  q  and  control  point  /'.  It  is  used  in  cubic  spline 
interpolation  method. 

■method 

Imethod  is  an  int  variable  and  denotes  the  interpolation  method  (1  =  cubic  spline,  2  = 
bspline,  3  =  4'*’  order  continuity  cubic  spline). 

tout_final 

tout  Jinal  is  a  double  variable  and  denotes  the  final  output  time. 

Nout 

Nout  is  an  int  variable  and  denotes  the  number  of  output  points. 

Nsamp 

Nsamp  is  an  int  variable  and  denotes  the  number  of  output  samples 

*Xout 

Xout  is  a  dMatrix  pointer  and  denotes  the  state  Xout[j][i]  for  each  time  tout[i].  The  result 
of  simulating  state  variables  is  stored  in  this  matrix. 
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*Uout 

Uout  is  a  dMatrix  pointer  and  denotes  the  input  Uout[j][i]  for  each  time  tout[i].  The  input 
which  is  applied  to  the  system  (real  or  virtual  model)  is  stored  in  this  matrix. 

*Yout 

Yout  is  a  dMatrix  pointer  and  denotes  the  output  YoutfJ][i]  for  each  time  tout[i].  The 
result  of  simulating  outputs  is  stored  in  this  matrix. 

*Zout 

Zout  is  a  dMatrix  pointer  and  denotes  the  flat  output  Zoiit[j][i]  for  each  time  toiitlij.  The 
result  of  simulating  flat  outputs  is  stored  in  this  matrix. 

*tout 

tout  is  a  dVector  pointer  and  denotes  the  output  time  with  spacing  at  the  interpolation 
points  dti. 

contructor  function 

RHC(double  tO,  double  T,  double  delta,  int  Nx,  int  Nu,  int  Nz,  int  Ny,  dVector  &x0, 
dVector  &u0,  int  Ns,  int  Ni,  int  Nc,  int  Nd,  double  tout_fmal,  int  Imethod,  int  ks) 

func_powell 

This  used  in  optimization  with  PoweF's  method. 

calc_J 

This  function  calculates  the  cost  function  for  trajectory  generation.  It  is  described  in 
3.3. 1.3  and  3. 3.2.2. 

optimize_cost 

This  function,  optimizes  the  cost  function  and  is  described  in  3. 3. 1.2  and  3. 3. 2. 2. 

interpolate 

This  function  does  the  interpolation  and  it  is  done  automatically  be  the  library. 

calculate_XU 

This  function  calculates  Xi  and  Ui  based  on  the  flat  outputs  Zi.  It  is  described  in  section 
3. 3. 1.3. 

calculate_U 

This  function  calculates  the  input  at  time  tc.  It  is  also  described  in  section  3. 3. 1.3. 

simulate_interval(double  tf,  double  dt) 

This  function  simulates  the  system  from  the  current  time  t  to  the  final  time  (/  using  the 
optimal  input  Ui  and  a  time  step  dt.  The  results  are  stored  in  the  output  variables  Xout, 
Uout,  Yout,  and  tout. 

calculate  xd 
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This  function  calculates  the  derivative  of  the  state  variables  (xd  =  f(x,u,t))  of  the  actual 
system.  It  is  used  in  simulation. 

calculate_outputs 

This  function  calculates  the  outputs  from  the  state  variables. 

save_data 

This  function  saves  variables  into  a  text  file.  It  is  described  in  section  3.3. 1 .2. 

save_flat_outputs 

This  function  saves  Hat  output  variables  over  the  current  interpolation  interval  into  a  text 
file. 


5.3.3. 1.2  programhclilocal.cpp'" 

This  file  includes  the  “main”  function  and  the  following  steps  are  done  in  it: 
a)  Variable  definition 

The  variables  which  are  used  in  RIIC  class  definition  are  defined  in  this  first  stage.  The 
basic  variables  can  be  classified  in  the  following  manner; 

•  Key  dimensions:  Nx,  Nu,  Nz,  Ny,  and  Nd 

•  Design  variables:  Ns,  Ni,  T,  delta,  Imethod,  and  ks 

•  Number  of  free  optimization  parameters:  Nc 

h)  Declaring  the  RHC  objects 

Using  the  variables  defined  in  the  first  part,  the  RHC  objects  are  defined.  We  can  define 
one  or  more  than  one  objects  here. 

c)  Setting  Initial  Conditions 

There  are  two  ways  to  apply  initial  conditions. 

•  One  way  is  to  apply  them  before  declaring  the  RIIC  objects.  In  this  case  the  initial 
conditions  should  be  applied  to  XO. 

•  The  other  way  is  to  apply  them  to  X,  the  current  state  vector,  after  declaring  the 
RHC  objects. 

d)  Optimizing  the  cost  function  over  one  horizon 

The  main  purpose  of  this  step  is  for  debugging  and  understanding  how  good  one 
optimization  horizon  is.  This  can  be  done  by  saving  the  data"  and  plotting  the  results  in 
the  Prediction  Horizon  (7).  Furthermore,  this  may  be  good  for  tuning  the  parameters  as 
well  as  good  for  checking  how  the  flat  outputs  describe  the  system. 


The  code  is  available  in  appendix  C. 

"  This  is  also  done  in  the  program  and  the  result  is  saved  in  “f.dat”.  The  command  line  in  the 
“program  heliJocaLcpp"  is:  rhcl  .  save_f  laL_outputs  (  "  f  .  dat "  )  ; 
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e)  Simulating  3 doj  helicopter  in  local  coordinates  using  RHC  approach 
For  simulation  two  main  functions  will  be  executed: 

•  rhcl .  optimi ze_cost  ( )  ; '^ 

•  rhcl . Simula te_interval (t, rhcl.dti/1000) ; 

The  first  one  is  the  main  function  which  performs  the  optimization  over  the  specified 
prediction  horizon  and  the  second  one  is  used  to  do  the  simulation. 

Before  applying  the  controller  to  the  real  apparatus  it  is  wise  to  test  it  on  a  virtual  system 
and  perform  some  basic  tuning.  This  is  done  by  using  the  second  function.  Therefore  the 
second  function  is  used  instead  of  applying  the  controller  to  a  real  apparatus. 

It  must  be  noted  that  the  optimization  is  done  over  each  execution  horizon  {delta) 
therefore  the  simulation  is  also  done  for  that  time.  Furthennore  the  user  can  specify  the 
actual  model  that  is  used  for  the  simulation  in  RFiC_user  (calculate_xd).  This  can  be 
useful  to  test  the  robustness  to  uncertainty.  The  dt  for  the  simulation  is  made  smaller  than 
dti  {TI{Ni-\))  to  get  a  more  accurate  simulation  using  Euler's  method  (here  dt  =  c/n71000). 


J)  Saving  the  simulation  output 

The  function  “save_data  ("file_name")  ”  saves  the  output  data  at  time  intervals  of 
dti'^  to  a  file.  The  data  is  saved  in  the  following  pattern: 

•  time  (first  column) 

•  states  (second  column  to  column  #  1+Nx) 

•  inputs  (column  #  2+Nx  to  #  1+Nx+Nu) 

•  flat  outputs  (column  #  2+Nx+Nu  to  #  1+Nx+Nu+Nz) 

•  outputs  (column  #  2+Nx+Nu+Nz  to  #  1+Nx+Nu+Nz+Ny) 


5. 3. 3. 1.3  RHC_uscr_hcli_local.cpp'‘* 

All  of  the  functions  that  need  changing  in  future  because  of  the  application  to  different 
apparatus  are  in  this  filel.  These  functions  will  be  explained  in  the  following: 

calculate_U 

The  input  of  this  function  is  Time  and  calculates  the  input  at  that  Time,  based  on  the 
inputs  which  are  calculated  from  the  optimization. 

From  optimization,  the  inputs  on  the  spline  interpolation  points  (spline  break  points)  are 
known,  so  the  inputs  are  known  only  in  some  points  and  it  is  not  a  continuous  function. 


'■  This  function  is  explained  in  section  3. 3. 2. 2. 

The  data  are  saved  at  each  interval  times  of  dti,  so  there  is  not  too  much  data. 
The  code  is  available  in  appendix  D. 
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calculateJU  perform  a  linear  interpolation'^  between  the  known  values  of  inputs  and  the 
inputs  will  be  continuous  functions  of  time. 

calculate_xd 

This  function  is  useful  for  simulation.  Using  J_heli_3doj2  function,  it  calculates  the 
derivatives  of  state  variables.  Note  that  by  using  a  model  in  helicopter  in  f_heli_3cloJ2 
function,  which  is  different  from  the  model  used  in  controller,  the  robustness  of  controller 
can  be  investigated. 

calculate_outputs 

This  function  calculates  the  outputs  from  the  state  variables. 

calculate_XU 

This  function  is  one  of  the  main  functions  in  this  file.  It  calculates  Xi  (state  variables)  and 
Ui  (inputs)  based  on  the  Zi  (flat  outputs).  Since,  some  of  the  state  variables  and  inputs  are 
calculated  from  equations  of  motions,  the  model  (equations  of  motion)  and  the  model 
parameters  are  included  in  this  function  and  must  be  changed  if  the  model  or  parameters 
change. 

calc_J 

This  function  is  the  most  important  function  in  this  tile.  It  calculates  the  cost  function  for 
trajectory  generation.  The  model  parameters,  all  of  the  constraint  including  the  initial 
conditions,  and  the  specifications  of  the  desired  trajectory  must  be  defined  here.  The 
input  of  this  function  is  a  dVector  containing  all  of  the  varying  control  points.  The 
following  steps  are  done  in  this  function: 

•  Getting  control  points  from  optimization  vector  c  (input)  and  forming  the  Cs 
vector  for  the  first  flat  output. 

•  Applying  initial  conditions  to  the  Cs  vector,  so  the  control  points  vector  of  the 
first  flat  output  will  be  completely  defined. 

•  Defining  boundary  conditions  of  the  spline 

o  ZiO:  boundary  condition  derivatives  (including  zero  derivative)  at  t=0 
o  ZiT—  boundary  condition  derivatives  (including  zero  derivative)  at  t=T 

•  Perfonning  the  foresaid  three  steps  to  the  other  flat  outputs 

•  Spline  interpolation  which  is  done  by  function  “interpolate”.  The  RHCOOL  will 
do  interpolation  itself  and  do  not  need  any  modifications. 

•  Calculating  Xi  and  Ui  based  on  Zi.  This  is  done  by  function  “calculate_XU” 
which  described  before. 

•  Integrating  cost  function  by  trapezoidal  integration  method.  The  cost  function  is 
defined  here.  Therefore  if  the  result  of  controller  is  not  satisfying  one  way  is 
changing  the  cost  function. 


f  hell  3dof2 


As  the  interpolation  points  are  usually  close  to  each  other,  the  result  of  linear  interpolation  is  acceptable. 
However,  it  is  also  possible  to  make  spline  interpolation  but  the  results  are  usually  the  same  while  linear 
interpolation  take  less  computation  time. 
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This  function  has  the  3DOF  model  of  helicopter  from  [6]  and  computes  the  derivatives  of 
the  state  variables.  This  function  is  described  completely  in  section  3.3.2. 


5.3.3.1.4  dV'ector.cpp'* 

This  file  defines  a  class  with  name  "'dVector".  dVector  defines  a  one  dimensional  array 
(vector)  based  on  dynamic  memory  allocation.  The  basic  format  for  defining  a  variable  in 
the  form  of  dVector  is: 

dVector  var(nl,n2) 

which  defines  a  variable  with  the  name  of  var.  var  is  a  one  dimensional  array  of  double 
variables,  nl  and  n2  are  indicating  the  first  and  last  elements  of  the  array,  respectively. 
Furthermore  var  is  initialized  and  all  of  its  elements  are  zero. 

For  referring  to  the  element  #n  the  following  command  must  be  used: 

var .e (n) 

The  benefits  of  using  dVector  are: 

•  It  uses  dynamic  memory  allocation  without  the  common  problems  of  using 
pointers. 

•  A  basic  one  dimensional  array  in  C++  starts  from  zero  index,  but  with  dVector  it 
can  be  started  from  any  integer,  including  positive  and  negative,  numbers. 

•  The  basic  mathematical  operations  like  adding,  subtracting  and  multiplying  are 
included  in  the  function. 


dMatrix.cpp’’ 

This  file  is  similar  to  the  dVecor  but  instead  of  one  dimensional  array  it  defines  a  2D 
array  (matrix).  This  file  defines  a  class  with  name  "dMatrix".  dMatrix  defines  a  2D  array 
based  on  dynamic  memory  allocation.  The  basic  format  for  defining  a  variable  in  the 
form  oi  dMatrix  is: 

dMatrix  var (nl , n2 , ml , m2 ) 

which  defines  a  variable  with  the  name  of  var.  var  is  a  two  dimensional  array  of  double 
variables,  nl  and  n2  are  indicating  the  first  and  last  rows  of  the  array,  respectively  and 
ml  and  m2  are  indicating  the  first  and  last  columns  of  the  array.  Furthermore  var  is 
initialized  and  all  of  its  elements  are  zero. 


The  code  is  available  in  appendix  E. 
’’  The  code  is  available  in  appendix  F. 


5-41 


Chapter  5:  Real-time  Scheduling  of  Multiple  Uncertain  Receding  Horizon  Control  Systems 


For  referring  to  the  element  in  row  #n  and  column  #m  the  following  command  must  be 
used: 

var . e (n , m) 

The  benefits  of  using  ciMatrix  are  the  same  as  dVector. 


dMatrix_array.cpp"' 

This  file  is  similar  to  the  ciMatrix  but  instead  of  2D  array  it  defines  a  3D  array.  This  file 
defines  a  class  with  name  Matrix _arr ay’" .  ciMatrixjarray  defines  a  3D  array  based  on 
dynamic  memory  allocation.  The  basic  format  for  defining  a  variable  in  the  form  of 
d Matrix _ar ray  is: 

dMatrix_array  var(kl,k2,nl,n2,ml,m2) 

The  other  specifications  are  the  same  as  ciMatrix. 


5.3.3.2  RHC  approach  of  3DOF  miniature  helicopter  using  RHCOOL 


In  this  section  the  3DOF  miniature  helicopter  is  controlled  using  RHCOOL.  First  the 
equations  of  motion  of  the  foresaid  helicopter  are  described,  and  then  the  programs  are 
explained. 


5.3. 3.2.1  Equations  of  motion  of  3DOF  miniature  helicopter 

The  equations  of  motion  of  a  6DOF  miniature  helicopter  are  derived  in  [6].  Figure  (20) 
illustrates  the  forces  and  moments  acting  on  the  helicopter  and  the  coordinate  system 
used  to  present  the  equations  of  motion.  For  3DOF,  the  equations  are  reduced  from  the 
original  6DOF  and  presented  in  the  same  body  coordinate  of  figure  (20): 


The  code  is  available  in  appendix  G. 
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/  2  ,  2  'T' 

u-vr - Cii\u  -fv - 1 


m 


m 


V  =  -iir - C,, 

m 


y/  =  r 


(38) 


where: 

^  y  ~  '2^“  ^ 

^ X  ~  Pit  ^ /us ^ 


(39) 


Figure  20-  Moments  and  forces  acting  on  the  6DOF  helicopter  and  the  used  body  coordinate 

frame  [6] 

Since,  in  this  example,  path  following  is  considered,  a  global  coordinate  system  is  also 
used.  For  simplicity,  the  coordinate  systems  used  for  3DOF  hovercraft  in  section  (3.1) 
(Figure  9)  arc  the  same  as  coordinate  systems  dctnicd  for  3DOF  helicopter.  The  extra 
equations  used  for  global  coordinate  is  as  follows: 

=  t/cos(^;/)  -  vsin(^^/) 

(40) 

=  trsin(v/) -I- v'cos(^y) 


Based  on  the  foresaid  equations,  the  following  assumptions  are  considered: 

•  u  ,  V,  r ,  y/ ,  X^,  and  Y^.  arc  considered  as  State  Variables. 

•  C,.  in  equation  (38)  is  supposed  to  be  zero.  This  means  that  the  friction  in  side 
direction  considered  to  be  zero  for  simplicity 
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•  Since  the  friction  in  side  direction  is  equal  to  zero,  u  and  y/  are  considered  to  be 
Flat  Outputs  [5]. 

Remark;  the  above  assumption  (C^,  =0)  has  the  benefit  that  only  two  flat  outputs  {ii 
and  (//)  are  enough  to  solve  the  problem.  Because  from  equation  (38),  v  =  -iir,  so  the 
state  variable  v  can  be  defined  by  integration  and  does  not  need  to  add  another  flat 
output  to  the  system.  If  C,  the  number  of  flat  outputs  will  be  3  (ii ,  v,  and  i^) 
which  increase  the  computation  time  about  50  percent. 


5.3.3.2.2  Example 

In  this  section  the  know-how  of  using  RHCOOL  is  described  by  implementing  RHC  to 
the  3DOF  helicopter.  The  reader  is  encouraged  to  read  the  section  3.3.1  especially  section 
3.3.1 .2.  Also  he/she  should  be  familiar  with  the  variable  definition  in  section  3.3.1 . 1 .  The 
following  steps  are  done  in  the  main  loop: 

Step  1 :  Calculating  the  inputs  by  optimizing  cost  function  (optimize_cosL) 

Step  2:  Simulating  the  results  (simulate_interval) 

Step  3;  Saving  the  results  (save_data) 

Each  function  (step)  is  described  bellow; 


Step  1:  optimize_cost 

The  RHC  will  be  executed  just  by  calling  optimize _cost"  in  the  main  function.  This 
function  must  be  in  a  loop  or  in  a  periodic  function.  By  executing  this  function  two  main 
tasks  will  be  done: 

•  Calling  “cfl/c_7”  which  is  a  function  for  calculating  cost  function  and  forming  the 
optimization  variables 

•  Performing  optimization  by  using  Powell’s  method 

Function  calc  J  is  a  key  point  in  the  library  and  the  following  four  basic  steps  arc  done 
for  calculating  the  cost  function 

Step  1:  defining  Cs  (control points  matrix)  from  endpoint  constraints  and  varying  control 
points 

The  user  does  this  and  then  RHCOOL  modifies  the  spline  control  points  appropriately. 


The  user  is  encouraged  to  read  calc_J  function  in  section  3.3.1 .3 
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For  all  of  the  control  points  there  is  a  matrix  (Cs).  Some  of  the  elements  of  this  matrix 
must  be  defined  by  constrains  of  the  problem  and  the  other  elements  arc  from  the  varying 
control  point  vector  which  is  the  input  of  calc_J. 

One  of  the  most  important  parts  that  the  user  has  to  do  is  making  a  vector  of  free  control 
pointsl).  The  parameters  of  this  vector  arc  the  output  of  optimization.  In  this  example,  we 
have  only  two  flat  outputs,  u  and  so  the  varying  control  point  vector  will  be  only 
from  these  two  variables.  This  vector  is  configured  such  that  the  first  elements  are  from  ii 
and  the  second  elements  arc  from  y/  {C  -[u  , y/ ] ). 

The  possible  constraints  that  considered  in  this  example  are  only  for  the  end  points.  So 
for  each  flat  output,  the  variable  and  its  derivatives  must  be  checked  at  the  start  point  and 
end  point  of  the  spline.  If  any  constraint  exists,  its  control  point  is  fixed,  and  if  there  is  no 
constraint,  its  control  point  will  be  a  varying  control  point.  For  clarity,  in  this  example  the 
first  control  point  of  u  will  be  fixed  (u  at  time  tO  has  constraint)  but  the  derivative  of  u 
docs  not  have  any  constrain  and  its  control  point  will  be  a  varying  one. 

Step  2:  spline  interpolation 

The  library  (RIICOOL)  does  this  automatically.  The  user  just  call  function  interpolate" 
after  defining  Cs. 

Step  3  :  calculate  the  State  variables  (Xi)  and  Inputs  (Ui)  based  on  Flat  outputs  (Zi) 

This  part  should  be  written  by  user  for  each  RHC  problem.  This  is  done  by  calling 
function  "calculate_XU". 

As  mentioned  in  section  (3. 3. 1.3),  calculate_XU  calculates  Xi  (state  variables)  and  Ui 
(inputs)  based  on  the  Zi  (flat  outputs).  Since,  some  of  the  state  variables  and  inputs  arc 
calculated  from  equations  of  motions,  the  model  (equations  of  motion)  and  the  model 
parameters  arc  included  in  this  function  and  must  be  changed  if  the  model  or  parameters 
change.  This  function  is  strait  forward  and  completely  depends  on  the  equations  of 
motion. 

Step  4:  integration  of  cost  function 

The  user  writes  this  function  to  Integrate  cost  function  by  trapezoidal  integration 
method^*^.  The  cost  function  is  also  defined  here.  Therefore  if  the  result  of  controller  is 
not  satisfying  one  way  is  changing  the  cost  function. 

Remark:  the  properties  of  the  reference  trajectory  also  must  be  defined  in  the  function 
calc_J.  In  this  example  an  ellipse  is  considered  as  the  reference  trajectory. 

Step  2:  simulate_interval 

In  this  example,  function  “simulate Jnterval"  is  used  to  simulate  the  results  the  time 
equal  to  execution  horizon^'.  Every  parts  of  this  function  will  be  done  by  the  library 


The  user  can  integrate  the  cost  function  by  other  methods  that  he/she  prefer  but  in  this  example 
trapezoidal  integration  is  used. 

See  section  3.3. 1 .2  for  more  information 
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except  the  modeling.  The  model  and  parameters  which  are  used  for  simulation  arc  in 
function  "'fjieli _3doj2" .  The  equations  of  motion  used  in  this  function  arc  equations  (38) 
and  (39).  In  addition,  the  parameters  are  the  same  as  parameters  defined  in  [6]. 


Step3:  Save_data 

The  results  are  saved  with  the  following  manner:  Time,  states,  inputs  and  flat  outputs. 
Please  sec  item  /’of  section  3.3.1 .2  for  detailed  information. 


5.3.4  Real-time  Scheduling  of  Hovercraft  Problem 


In  this  section,  the  proposed  method  described  in  Section  5. 2. 2. 2  is  applied  to  RIIC 
stabilization  of  two  hovcrcrafts  using  a  single  computer.  Two  models  are  considered  in 
this  section;  one  model  deals  with  only  rotation  direction  and  the  other  has  the  3 DOF 
hovercraft. 

Rotation  direction  only 

In  this  case  inputs  to  the  motors  of  hovercraft  arc  the  same  but  in  the  opposite  direction. 
Also  from  identification  procedure  the  coefficients  Oj  and  of  equations  (30)  was 
almost  the  same.  Therefore  the  equations  of  motion  for  hovercraft  #  i  can  be  estimated  as 
follows: 


n  =  Kd'  +  2a. 


=  n 


(41) 


Among  the  available  RHC  methods,  the  quasi-infinite  RIIC  approach  described  in  [1  1]  is 
implemented.  The  state  variables  arc  and  r.  and  the  flat  outputs  [5]  arc  v', .  The 

prediction  horizon  is  selected  as  1  seconds  and  the  weighting  matrices  Q  and  R  arc  taken 
as  identity  matrices  for  both  subsystems.  The  matrix  P  in  (4)  is  found  by  solving  the 
Lyapunov  equation  (9)  in  reference  [11].  The  experimental  results  will  be  presented  in 
section  4. 


Full  3DOF  Hovercraft 

The  equations  of  motion  are  from  equations  (28)  to  (30)  and  equations  (40)  for  both 
apparatus.  Therefore  for  subsystem  #  i  the  following  equations  are  obtained: 

v.=-i,_r^+h;,v.  (42) 
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AV  cos(^i/,.)-v,  sin(v/;) 

=ty;Sin(v/,)  +  V;COs(^i/J 

The  state  variables  are  tr,  ,  v^,  r, ,  [j/^,  A'^ , ,  and  .  Among  the  available  RHC  methods, 

the  quasi-infinite  RHC  approach  described  in  [11]  is  implemented.  The  prediction 
horizon  is  selected  as  1  seconds  and  the  weighting  matrices  Q  and  R  are  taken  as  identity 
matrices  for  both  subsystems.  The  matrix  P  in  (4)  is  found  by  solving  the  Lyapunov 
equation  (9)  in  reference  [11]. 

As  discussed  in  section  3.3.2  the  h\^  is  considered  to  be  zero.  This  assumption  has  this 

benefit  that  the  number  of  flat  outputs  is  reduced  to  two  for  each  subsystem  ( and  ). 

By  this  assumption,  ,  so  the  state  variable  v,  is  calculated  by  numerical 

integration'".  If  6',  is  not  considered  to  be  zero,  another  fiat  output  (v; )  has  to  be  added 

and  the  number  of  flat  outputs  will  be  increased  to  3  which  increase  the  computation  time 
about  50  percent. 

The  experimental  results  will  be  presented  in  section  4. 


In  section  3.3.2  trapezoidal  integration  was  used  for  3DOF  helicopter. 


5-47 


Chapter  5:  Real-time  Scheduling  of  Multiple  Uncertain  Receding  Horizon  Control  Systems 


5.4  Experimental  Verification 


In  this  section  the  result  of  applying  RHC  to  single  and  multiple  hovcrcrafts  is 
investigated.  It  should  be  mentioned  that  in  the  section  3.3.2,  a  3DOF  helicopter  model  is 
solved  by  using  the  library  in  order  to  explain  library  and  to  notify  the  reader  that  this 
library  is  applicable  to  different  apparatus.  One  of  the  future  applications  of  this  library  is 
to  apply  RHC  to  multiple  6DOF  helicopters  and  the  example  of  3DOF  helicopter  in 
addition  of  explaining  the  library,  notify  the  applicability  of  the  library  to  helicopters. 

Furthermore,  the  equations  of  motion  for  a  3DOF  helicopter  arc  similar  to  equations  of 
motion  of  hovercraft.  Let’s  explain  this  similarity  more.  Equations  (25)  to  (27)  arc  for 
hovercraft  and  equations  (38)  are  for  3DOF  helicopter.  In  these  equations  there  are  two 
differences: 

1-  The  friction  terms  arc  different  which  can  be  modified  easily  in  the  code  and  also 
in  low'  velocities  the  differences  arc  not  so  much. 

2-  The  inputs  to  the  system  are  different  but  with  a  transformation  matrix  they  easily 
can  be  changed  to  each  others  as  follows: 


-  m 

-tn 

\T  1 

rF,i 

T 

tr 

—• 

-llzz 

(43) 

[  2JI„. 

2Jl.r  J 

But  the  state  variables  are  the  same.  In  addition,  all  of  the  tests  done  in  this  report  are 
with  low  velocities,  so  for  the  considered  model  in  the  controller,  the  friction 
coefficients  are  considered  to  be  zero. 

In  the  following  sections,  first,  the  apparatus  is  described  and  every  component  is 
explained,  after  that  the  open  loop  response  is  presented.  Then  the  results  of  RHC  for  a 
single  hovercraft  and  multiple  hovcrcrafts  are  presented. 


5.4.1  Description  of  the  Apparatus 


Figure  21  shows  a  schematic  diagram  of  the  setup.  The  setup  consists  of  the  following 

IT 

parts"  : 

1-  A  set  of  cameras  (Cl  to  C4)  used  to  capture  images  and  to  forward  to  the  Vision 
computers  (VI  to  V4).  Each  image  consists  of  some  colored  squares  and  the 


The  experimental  setup  which  is  explained  here,  is  the  total  setup  prepared  in  the  CIS  Lab.  However, 
some  parts  of  the  setup  were  used  in  each  experiment. 
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computers  calculate  the  centroid  of  each  square  (Figure  22).  Each  camera  is 
connected  to  one  eomputer  in  order  to  decrease  the  image  processing  time. 

2-  All  of  the  centeroids  calculated  by  the  vision  computers  are  transferred  to  the 
server  via  a  Gigabyte  Ethernet  Switch.  The  server  combines  the  centroid 
information  and  sends  them  to  the  main  controller  computers  (Ml  and  M2)  via 
another  Gigabyte  Ethernet  Switch. 

3-  Ml  and  M2  receive  the  data  from  sensors.  One  sensor  is  the  vision  system  and  the 
data  is  sent  from  the  server,  and  the  other  sensor  is  a  wireless  orientation  sensor 
(Wireless  InertiaCube3  [24])  and  calculate  the  input.  The  input  signals  are 
transferred  to  the  apparatus  via  data  acquisition  board  and  wireless  transmitter'"' 
(T1  and  T2).  In  this  setup  each  computer  is  connected  to  two  apparatuses,  at  most. 
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Figure  21-  Schematic  layout  of  e.xperimental  setup 


The  wireless  transmitter  is  “HiTec  Laser  6  Channel  75MHz  RC  Controller  Pkg”  and  is  e.xplained  in  [2K] 
and  [31], 
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Figure  22-  The  colored  squares  used  to  measure  position  of  apparatus  by  vision  system 

The  apparatus  used  in  this  investigation  is  a  miniature  hovercraft  and  is  shown  in  Figure 
22.  This  hovercraft  is  a  modified  version  of  the  RVC  hovercraft  [32],  The  motors  arc 
placed  with  more  distance  from  each  other  to  produce  a  larger  torque.  In  addition,  a 
voltage  amplifier  board  with  a  potentiometer  is  added.  The  voltage  amplifier  board  is 
from  an  R/C  motor,  model  IIS-700BB  [33]. 

For  controlling  the  apparatus,  a  wireless  transmitter  is  attached  to  the  computer  using  a 
data  acquisition  board  and  the  receiver  is  attached  to  the  apparatus.  In  order  to  connect 
the  transmitter  to  the  data  acquisition  board  some  modifications  are  done  in  the 
transmitter.  The  data  acquisition  board,  transmitter  and  all  of  the  modifications  are 
described  in  [28].  Two  batteries  are  used  as  follows: 

1-  One  9.6  V  battery  used  for  the  main  fan  which  cause  flouting  of  hovercraft.  This 
fan  pushes  air  under  the  hovercraft  and  causes  it  to  move  on  a  film  of  air, 
therefore  the  friction  of  hovercraft  in  all  directions  (forward,  side,  and  angular)  is 
very  small. 

2-  One  6  V  battery  used  for  both  propellers.  This  battery  is  connected  to  the  receiver 
and  both  propeller  motors  are  connected  to  the  receiver. 

As  mentioned  above,  in  order  to  measure  orientation  and  angular  velocity  of  miniature 
hovercraft,  a  wireless  sensor  is  used.  This  sensor  is  a  Wireless  InertiaCube3  from 
InterSence  [24].  The  position  in  X-Y  coordinate  is  obtained  using  Vision  System  which  is 
developed  in  the  CIS  Lab.  Theses  two  sensors  are  illustrated  in  the  following  two 
sections. 
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lncrtiaCube3  Sensor 


In  order  to  have  orientation  and  angular  velocity  of  miniature  Hovercraft  and  miniature 
Helicopter,  Wireless  InertiaCube3  from  IntcrSencc  has  been  used.  This  section  describes 
the  sensor. 


5.4.1. 1.1  Principles  of  lncrtiaCubc3 

The  lnertiaCubc3  is  an  inertial  3-DOF  (Degree  of  Freedom)  orientation  tracking  system. 
It  obtains  its  motion  sensing  using  a  miniature  solid-state  inertial  measurement  unit, 
which  senses  angular  rate  of  rotation,  gravity,  and  earth  magnetic  field  along  three 
perpendicular  axes.  The  angular  rates  arc  integrated  to  obtain  the  orientation  (yaw,  pitch, 
and  roll)  of  the  sensor.  Gravitometer  and  compass  measurements  are  used  to  prevent  the 
accumulation  of  gyroscopic  drift  [25]. 

The  lnertiaCube3  is  a  monolithic  part  based  on  micro-clectro-mechanical  systems 
(MEMS)  technology  involving  no  spinning  wheels  that  might  generate  noise,  inertial 
forces,  and  mechanical  failures.  The  InertiaCubc  simultaneously  measures  9  physical 
properties,  namely  angular  rates,  linear  accelerations,  and  magnetic  field  components 
along  all  3  axes.  Micro-miniature  vibrating  elements  arc  employed  to  measure  all  the 
angular  rate  components  and  linear  accelerations,  with  integral  electronics  and  solid-state 
magnetometers.  The  magnetometers  are  included  for  optional  yaw  drift  correction  in  the 
sourceless  inertial  orientation  mode  only.  (See  Figure  23)  [25] 

Figure  24  illustrates  the  basic  physical  principal  underlying  all  Coriolis  vibratory  gyros. 
Suppose  that  the  tines  of  the  tuning  fork  are  driven  by  an  electrostatic,  electromagnetic, 
or  piezoelectric  drive  to  oscillate  in  the  plane  of  the  fork.  When  the  whole  fork  is  rotated 
about  its  axis,  the  tines  will  experience  a  Coriolis  force  F  -d>xV  pushing  them  to  vibrate 
perpendicular  to  the  plane  of  the  fork.  The  amplitude  of  this  out-of-planc  vibration  is 
proportional  to  the  input  angular  rate,  and  is  sensed  by  capacitive,  inductive,  or 
piezoelectric  means  to  measure  the  angular  rate  [25]. 
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X 


Figure  23  -  Functional  diagram  of  IncrtiaCubc3  [25] 


CO 


Figure  24-  Principle  of  Coriolis  vibratory  Gyroscope  [25] 

5.4. 1.1. 2  Experimental  data 

As  mentioned  above,  the  InertiaCube3  can  get  orientation  and  angular  velocities  in  3D 
and  present  the  information  in  the  form  of  Euler  Angles  (Yaw,  Pitch,  and  Roll).  For  a 
3DOF  hovercraft,  only  information  in  Yaw  direction  will  be  used.  To  prevent  drift 
problem,  this  sensor  is  used  with  full  compass  mode  and  in  this  case  any  metallic  objects, 
magnetic  or  electric  field  can  cause  inconsistency  in  sensor  readings  if  they  arc  in  the 
vicinity  of  the  sensor. 
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In  order  to  know,  how  this  sensor  is  working  in  CIS  Lab  (the  area  which  is  prepared  for 
the  vision  system),  some  visual  tests  have  been  done  and  it  was  obvious  that  the  sensor 
reading  was  not  reliable  and  was  changing  in  an  arbitrary  manner.  So,  the  following  tests 
have  been  done. 

The  sensor  was  moving  along  each  line  of  B1  to  B5  (Figure  25  and  Figure  26)  while  was 
being  kept  in  one  direction.  When  this  sensor  was  so  close  to  the  tloor,  the  changing  was 
more  than  having  a  higher  level.  So  these  tests  were  performed  in  three  different  levels. 


Figure  25-  Used  layout  for  getting  data  in  CIS  Lab 
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Figure  28-  changing  in  sensor  reading  of  Yaw  direction  when  was  kept  3.5  cm  over  the  floor 

In  Figure  28,  sensor  was  kept  in  one  level  (3.5  cm  above  the  floor)  and  in  the  same 
direction  and  was  following  each  lines  of  B1  to  B5.  If  the  area  was  perfect  and  without 
any  magnetic  field  (except  the  Earth  magnetic  field),  all  of  the  data  from  sensor  should  be 
the  same. 
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In  this  level,  the  sensor  was  attached  to  a  wood  beam  on  top  of  a  plastic  box  and  has  65 
cm  distance  from  the  floor  (see  Figure  29).  The  data  arc  shown  in  Figure  30. 
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Figure  29-  Attachment  of  the  sensor  to  a  wooden  beam  on  top  of  the  plastic  box  in  level  2 
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Figure  30-  changing  in  sensor  reading  of  Yaw  direction  when  was  kept  65  cm  above  the  door 
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In  Figure  30,  sensor  was  kept  in  one  level  (65  cm  above  the  floor)  and  in  the  some 
direction  and  was  following  each  lines  of  B1  to  B5.  If  the  area  was  perfect  and  without 
any  magnetic  field  (except  the  Earth  magnetic  field),  all  of  the  data  from  sensor  should  be 
the  same. 
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Level  3 


In  this  level,  the  sensor  was  attached  to  a  wood  beam  on  top  of  a  plastic  box  and  has 
1 25.5  cm  distance  from  the  floor  (sec  Figure  3 1 ).  The  data  arc  shown  in  Figure  32. 


Figure  31-  Attachment  of  the  sensor  to  a  wooden  beam  on  top  of  the  plastic  box  in  level  3 
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Figure  32-  changing  in  sensor  reading  of  Yaw  direction  when  was  kept  1 25.5  cm  above  the  floor 


In  this  case  that  changing  is  not  too  much,  and  it  is  possible  to  find  an  area  with 
acceptable  performance.  So  the  data  in  A2  to  A5  direction  were  recorded  as  well.  (Figure 
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Figure  33-  changing  in  sensor  reading  of  Yaw  direction  when  was  kept  125.5  cm  over  the  tioor 


5. 4.1. 1. 3  Discussion 

From  Figure  32  and  Figure  33  it  can  be  shown  that  there  exist  some  areas  that  this  sensor 
can  be  used  in.  In  the  areal  which  is  shown  in  Figure  34,  the  changes  in  sensor  data  are 
limited  in  10  degrees.  Also  arca2  in  Figure  35  is  a  bigger  area  which  changes  arc  not 
more  than  1 5  degrees. 
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Figure  34-  area  which  changes  in  sensor  data  is  less  than  10  degrees 
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Figure  35-  area  which  changes  in  sensor  data  is  less  than  15  degrees 


So  one  way  of  working  with  this  sensor  in  CIS  Lab  with  Full  Compass  Mode,  is  having  a 
table  made  of  non-metallic  materials  with  the  height  of  about  125  cm. 


The  other  solution  can  be  shielding  the  area  or  using  a  larger  magnet  in  order  to  make  a 
unidirectional  magnetic  field  in  the  area  of  working. 

The  other  solution  is  to  use  the  sensor  “without  Compass”  or  “Partial  Compass  Mode” 
(see  [25]  for  more  information).  In  this  case  the  drift  problem  in  the  reading  values  of 
Yaw  should  be  corrected  with  another  solution.  Based  on  this  solution,  the  combination 
of  wireless  InertiaCubeS  and  vision  system'^  is  used  in  sections  4.3  and  4.4.  So  the 
angular  velocity  was  measured  by  wireless  IncrtiaCubc3  and  the  angle  was  obtained  from 
vision  system. 

Another  thing  which  I  should  mention  here  is  that  the  wireless  lncrtiaCube3  is  not 
sensitive  to  changing  of  the  hovercraft  batteries  (The  IncrtiaCube2  was  sensitive  to  this 
matter  and  after  changing  the  batteries  it  needed  about  half  an  hour  being  in  the  new 
situation  to  work  properly). 


5.4.1.2  Vision  Array 

In  order  to  have  the  position  (X-Y)  of  the  hovercraft  a  vision  array  is  built  at  a  height  of 
3.5  meters  in  the  test  area  of  the  CIS  Lab.  It  produces  a  test  area  in  the  shape  of  a 
rectangle  with  dimensions  of  4  by  5  meters.  4  webcams'^  arc  connected  to  4  computers 


See  section  4.1.2  for  more  information. 

Installing  more  webcams  is  also  possible  but  with  four  webcams  the  working  area  is  covered  with  a 
reasonable  resolution. 
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and  they  send  the  data  to  a  main  computer  which  acts  as  a  server.  See  Figure  21,  and 
Figure  36  to  Figure  38.  Figure  36  shows  an  overview  of  the  vision  array.  Figure  37  is  a 
schematic  drawing  showing  a  connection  point.  Figure  38  shows  a  connection  point  and  a 
webcam. 

Each  computer  with  a  webcam  connected  to  it  can  get  the  picture  and  process  the  image 
within  less  than  0.05  of  a  second.  Therefore  the  data  from  the  whole  vision  system  is 
updated  in  less  than  0.05  of  a  second. 


Figure  36-  An  overview  of  the  vision  array 
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5.4. 2.1  Forward  .Movement 

In  this  case  a  same  input  signal  applied  to  both  motors  {u^  =«,  in  equation  (28)).  The 
results  are  shown  in  Figure  39  for  different  cases  of  the  following  input  signals, 
iq  -U2  =0.5,  0.7,  0.85,  and  1.0 
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Figure  39-  forward  velocity  (u)  of  hovercraft  caused  by  different  input  signal  values 
The  data  used  in  Figure  39  are  filtered  as  described  in  section  3.2. 

5.4.2. 2  Rotation 


In  this  case  a  same  input  signal  but  in  reverse  direction  applied  to  both  motors  {l4^  =  -it 2 
in  equation  (30)).  The  results  are  shown  in  Figure  39  for  different  cases  of  the  following 
input  signals. 

u^  -  -112  =  0.5,  0.7,  0.85,  and  1 .0 
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Figure  40-  Angular  velocity  ( [j/ )  of  hovercraft  caused  by  different  input  signal  values 


In  Figure  40  Wireless  lnertiaCube3  is  used  to  get  data,  so  the  data  are  not  filtered  and 
they  are  angular  velocities  measured  hy  sensor. 


5.4.3  RHC  of  a  Single  Hovercraft 

In  order  to  get  the  results  of  one  hovercraft  controlled  hy  RHC  approach,  a  setup  used 
which  is  shown  in  Figure  41.  The  RHC  method  used  in  this  report  is  based  on  the  method 
presented  in  [6],  the  sensor  is  wireless  IncrtiaCubc3  and  the  control  parameters  arc  as 
follows: 


Nx  =  2 

Number  of  states  and  \j/) 

Nz=  1 

Number  of  flat  outputs  ( ^ ) 

Nu  =  1 

Number  of  inputs  ( t/, 

Ny  =  2 

Number  of  outputs  and  y/) 

Nd  =  2 

Maximum  number  of  derivatives  from  flat  outputs 

Ns  =  6 

Number  of  Spline  control  points 

Ni  =  51 

Number  of  Spline  interpolation  points 

See  equation  (41)  for  clarity.  Note  that  only  one  hovercraft  is  used  at  this  step. 
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T  =  3.0  (sec) 

Prediction  Florizon 

delta  =  Ts  =0.05  (sec) 

Execution  Horizon  which  is  equal  to  the  period  of  the  periodic 
function  (Ts)  defined  for  real  time  execution 

Nc  =  Ns 

Number  of  varying  control  points 

1  method  =  1 

It  means  that  the  cubic  spline  is  used  for  interpolation 
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Figure  41-  Schematic  layout  of  experimental  setup  for  one  hovercraft  and  one  camera"* 

For  single  hovercraft  two  groups  of  tests  arc  performed  only  in  rotation  direction;  in  the 
first  one,  the  vision  system  docs  not  used  and  all  of  the  data  were  measured  by  Wireless 
InertiaCube3  with  the  partial  compass  mode.  The  other  is  done  were  the  combination  of 
vision  system  and  wireless  IncrtiaCubc3  was  used. 

5.4.3. 1  Rotation  Direction  with  wireless  lncrtiaCubc3 

In  this  case  two  tests  have  been  done,  Regulation  and  Tracking. 


Regulation  with  RHC 

In  this  case,  the  hovercraft  was  started  with  a  Yaw  angle  which  was  not  equal  to  zero  and 
with  the  RllC  Regulating  controller  should  go  back  to  zero  angle. 

The  result  is  shown  in  Figure  42. 


Cl,  VI,  Ml,  ri,  and  HI  are  described  in  the  description  of  Figure  21. 
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Figure  42-  regulating  Yaw  angle  of  the  hovercraft  with  RHC  controller 


As  shown  in  Figure  42,  the  angle  of  hovercraft  is  getting  back  to  zero  by  the  RHC 
regulator. 


Tracking  with  RHC 

In  this  case,  the  Yaw  angle  of  hovercraft  should  follow  a  trajectory.  The  trajectory  was 
sinusoidal  with  respect  to  time.  The  result  is  shown  in  Figure  43. 
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Figure  43-  Yaw  angle  tracking  of  the  hovercraft  with  RHC  controller  when  only  wireless 

lncrtiacube3  is  used 
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As  shown  in  Figure  43,  the  angle  of  hovercraft  (red  line)  is  following  the  reference 
trajectory  (blue  line).  As  mentioned  before  the  sensor  is  used  in  the  partial  compass 
mode,  and  it  had  drift,  so  it  followed  the  trajectory  but  not  very  well  in  Figure  43.  In 
addition,  in  reality  the  zero  angle  of  hovercraft  was  changing  because  of  the  drift  and  it  is 
not  shown  in  the  Figure. 

In  the  next  section  the  data  from  Vision  System  is  added  and  the  performance  of  the 
system  is  better. 

5.4.3. 2  Rotation  Direction  with  wireless  lncrtiaCube3  and  V  ision  System 

In  Figure  44  the  angular  velocity  )  is  measured  by  wireless  lncrtiacubc3  and  the  angle 
is  measured  using  the  vision  system  and  the  hovercraft  is  able  to  follow  that  better.  The 
green  line  shows  the  xj/  angle  measured  by  wireless  IncrtiaCubc3  where  the  drift  in  the 
measurement  is  obvious. 
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Figure  44-  Yaw  angle  traeking  of  the  hovereraft  with  RHC  controller,  when  xp  is  measured  by 
vision  system  and  ^  is  measured  by  wireless  lnertiaCube3  (the  green  line  shows  the  Xj/  angle 
measurement  by  lnertiaCubc3  where  the  drift  in  the  measurement  is  obvious) 


In  the  next  test  only  vision  system  is  used.  So  the  angle  and  angular  velocity  of  hovercraft 
measured  and  calculated  by  data  from  vision  system.  By  using  vision  system,  only  the 
angle  of  hovercraft  can  be  measured,  and  the  angular  velocity  was  calculated  by  finite 
difference  method,  and  filtering  the  data.  Consider  equation  (36)  sNxXh  filter  value  equals 
to  5.  So  the  measured  data  in  5  steps  before  the  current  state  were  used  to  calculate 
angular  velocity  of  current  state.  This  filter  value  was  obtained  by  trial  and  error.  In  this 
case  with  the  real  model  parameters,  the  system  was  not  working  and  only  when  the  ^3 
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value  in  equation  (41)  was  selected  4  times  greater  than  the  actual  value,  the  system  was 
stable  and  worked.  The  data  are  shown  in  Figure  45  and  Figure  46. 
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Figure  45-  Yaw  angle  tracking  of  the  hovercraft  with  RHC  controller,  when  if/  is  measured  by 
vision  system  and  if/  is  calculated  from  the  data  of  vision  system. 


As  shown  in  the  Figure  45  the  tracking  is  not  very  good  because  the  model  used  in 
prediction  of  states  (the  model  used  in  controller)  was  not  accurate  which  explained 
before. 


Next  Figure  shows  the  angular  velocity  measured  by  lnertiaCubc3  and  the  angular 
velocity  calculated  from  vision  data.  It  can  be  concluded  from  Figure  46  that  the 
calculated  data  from  vision,  has  about  0.2  seconds  delay  from  the  values  measured  by 
wireless  InertiaCubeS.  It  is  noticeable  because  in  this  test,  which  vision  system  is 
involved,  two  periodic  functions  were  defined  to  do  two  time  consuming  jobs  (reading 
vision  data  and  calculating  the  outputs  (RHC)).  The  period  of  the  first  task  (reading 
vision  data)  is  0.03  seconds  and  the  period  of  the  second  task  is  0.05  seconds.  Since  the 
data  from  5  steps  before  (0.15  seconds  before)  are  used  as  well  as  the  data  in  the  current 
time,  some  part  of  this  delay  is  expectable. 
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Figure  46-  yj/  measured  by  wireless  InertiaCubeB  (blue  line)  and  \j/  ealeulated  from  vision  data 
(red  line).  A  delay  in  the  red  line  can  be  concluded  comparing  to  the  blue  line. 


5.4.4  RHC  of  two  Hovercrafts 

In  order  to  get  the  results  of  two  hovererafts  eontrolled  by  RHC  approaeh,  the 
experimental  setup  illustrated  in  Figure  47  is  used.  For  simplicity  only  one  camera  is 
used,  and  two  hovercrafts  arc  controlled  by  one  computer.  One  of  the  hovererafts  uses 
both  data  from  vision  and  wireless  lncrtiaCubc3  (subsystem  #1),  and  the  other  one  uses 
only  data  from  vision  (subsystem  #2). 

Three  periodic  tasks  arc  defined  in  this  experiment.  One  for  reading  vision  data,  one  for 
controller  #1,  and  the  other  for  controller  #2.  For  solving  RHC  problem  for  both 
subsystems,  RHCOOL  is  used  and  the  control  parameters  arc  the  same  as  controller  for 
one  system  (data  presented  for  controller  in  the  beginning  of  section  4.3).  The  only 
difference  is  the  execution  horizon  (or  the  period  of  the  periodic  task).  The  second 
subsystem,  which  has  more  uncertainty  because  of  using  vision  data  for  angular  velocity 
calculation,  has  the  execution  horizon  of  0.05  seconds  while  for  the  first  subsystem  the 
execution  horizon  is  0.10  seconds.  Only  one  model  is  used  for  the  controllers  of  both 
subsystems.  As  discussed  before,  the  model  parameter  a-^  in  equation  (41)  considered  to 
be  about  four  times  greater  than  the  actual  value  because  of  the  problem  in  angular 
velocity  calculated  by  vision  data. 
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In  order  to  illustrate  that  the  performance  does  not  decreased  when  one  computer  is  used 
for  both  hovercrafts,  the  result  of  tracking  with  the  same  model  parameter  when  one 
computer  controls  only  one  hovercraft,  is  also  presented. 
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Figure  47-  Schematic  layout  of  experimental  setup  for  two  hovercrafts,  one  camera,  and  one 

wireless  lnertiaCube3  sensor""* 

In  Figure  47  the  computer  used  for  controlling  two  hovercrafts  (Ml)  is  a  Pentium  4,  with 
one  CPU  of  2.8  GHz  speed.  This  is  the  same  as  computer  used  in  Figure  41  for  one 
hovercraft. 

As  two  vehicles  are  controlled  with  a  single  computer,  the  scheduling  approach  described 
in  section  3.4  is  used.  The  equations  of  motion  of  two  hovercrafts  arc  equations  (41 )  with 
the  following  parameters: 

First  hovercraft  (/=  1 ): 
b\  -i  --0.15 

rr,3--1.7 

And  the  second  hovercraft  (/=2): 

a;  3  =-0.11 

^2,3  ~  ^ 

But  the  parameters  used  in  controller  arc  as  follows: 

Ky  =  b[y  =  0 
<^1.3  =  ^2.3  ^  “^-5 


Cl,  VI ,  Ml,  Tl,  H 1 ,  and  112  are  described  in  the  description  of  Figure  21 . 
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As  discussed  above,  the  model  parameters  are  changed  because  of  the  problem  in 
calculated  angular  velocity  of  the  second  subsystem  from  vision  data.  Also  only  one 
model  is  used  in  the  eontroller  of  both  subsystems.  The  results  are  as  follows; 


scheduling  of  two  subsytems  on  one  computer 


\ 

• 

1 

j  v  subsystem  #1  (deg) 

v  subsystem  #2  (deg) 

1  Reference  Angle  (deg) 

-60'  ‘  ‘  '  -  .  ■  ' 

0  5  10  15  20  25  30  35 


Time  (s) 


Figure  48-  Yaw  angle  traeking  of  the  first  and  second  hovercrafts  (red  and  green  lines 
respectively)  and  the  reference  path  (blue  line)  (after  scheduling) 


v(/'  from  InertiaCubeS  and  v  from  vision  system 
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Figure  49-  Yaw  angle  tracking  of  the  first  hovercraft  (red  line)  and  the  reference  path  (blue  line)  - 
only  first  hovercraft  with  the  same  computer  and  model  parameters  (before  scheduling) 
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Figure  50-  Yaw  angle  tracking  of  the  second  hovercraft  (red  line)  and  the  reference  path  (blue 
line)  -  only  second  hovercraft  with  the  same  computer  and  model  parameters  (before  scheduling) 


Discussion 

By  comparison  of  Figures  48  to  50,  the  performance  of  the  system  when  scheduling  is 
performed  docs  not  change.  Since  the  model  parameters  arc  not  accurate,  the 
performance  of  both  subsystems  is  not  very  good  even  when  they  are  controlled 
individually  (only  one  hovercraft  in  one  computer).  However  in  Figure  44  the  hovercraft 
was  controlled  very  well,  individually  and  followed  the  same  path,  perfectly.  Because  it 
was  using  wireless  IncrtiaCubc3  for  measuring  angular  velocity  and  the  model 
parameters  were  accurate.  Since  only  one  wireless  IncrtiaCubc3  was  available,  the 
scheduling  of  two  hovercrafts  was  done  in  the  foresaid  manner  (one  with  wireless 
InertiaCubc3  and  the  other  with  vision  data  for  angular  velocities).  Therefore  the  model 
parameters  were  changed  (in  order  to  be  able  to  work  with  angular  velocities  calculated 
from  vision  data)  and  the  tracking  was  not  very  good.  Some  videos  arc  prepared  showing 
the  scheduling  of  two  subsystems  with  one  computer  where  following  the  defined  path 
(only  yaw  angle  is  following).  Video  1,  Video2,  and  Video3. 

In  all  of  the  following  clips,  the  reference  path  is  a  sinusoidal  wave  versus  time  with  the 
amplitude  of  45  degrees. 

reference  path  =  45  x  sin(/a>) 

In  deferent  clips  the  frequency  of  this  wave  (<y)  was  changed. 
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Video! : 

w  =  0.3  The  frequency  of  the  reference  path 

Ts1  =  0.15  The  period  of  the  first  subsystem 

Ts2  =  0.05  The  period  of  the  second  subsystem 

Ts3  =  0.03  The  period  of  the  reading  data 

Video2: 


w  =  0.5 

The  frequency  of  the  reference  path 

Tsi  =  0.15 

The  period  of  the  first  subsystem 

Ts2  =  0.05 

The  period  of  the  second  subsystem 

Ts3  =  0.03 

The  period  of  the  reading  data 

Video3: 


w  =  0.7 

The  frequency  of  the  reference  path 

Tsi  =  0.15 

The  period  of  the  first  subsystem 

Ts2  =  0.05 

The  period  of  the  second  subsystem 

Ts3  =  0.03 

The  period  of  the  reading  data 
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5.5  Conclusions  and  Future  Work 

5.5.1  Conclusions 

In  this  report,  a  systematic  approach  is  developed  for  scheduling  computation  and 
determination  of  the  execution  horizon  for  multiple  uncertain  RHC  systems.  It  was 
shown  that  using  a  rate  monotonic  priority  assignment  method  combined  with  analytical 
bounds  on  the  prediction  error,  the  problem  of  scheduling  multiple  uncertain  plants  can 
be  cast  into  an  appropriate  constrained  optimization  problem.  The  constraints  guarantee 
that  the  processes  will  be  schcdulablc,  and  the  optimization  provides  performance 
robustness  to  uncertainty.  The  proposed  method  was  first  applied  via  Matlab  to  a  double 
integrator  example  problem  demonstrating  the  validity  of  the  approach.  Performance  of 
the  method  was  also  demonstrated  by  applying  it  to  some  sets  of  under  actuated  miniature 
hovercrafts  in  a  hard  real  time  environment.  The  latter  was  done  by  developing  a  RHC 
Object  Oriented  Library  (RHCOOL).  This  report  was  explaining  Task  #7  and  Task  #8  of 
the  first  proposal. 


5.5.2  Future  Work 

1-  Design  a  scalable,  dynamic  run-time  scheduling  algorithm  for  multiple  RHC  running 
on  distributed  digital  processors  (Task  II) 

Dynamic  scheduling  of  computational  and  communication  resources  for  multiple  RHC 
control  problems  is  very  important  since  scheduling  requirements  and  resulting 
performance  can  vary  dramatically  as  the  system  dynamically  varies  over  time.  This  can 
be  achieved  by  formulating  the  uncertainty  bounds  as  a  function  of  state,  time,  external 
inputs,  and  other  factors  that  infiuence  the  local  properties  of  these  bounds.  These  time 
varying  bounds  can  then  be  optimized  on-line  as  the  system  is  varying. 

Finally,  the  optimization  domain  will  be  expanded  to  include  a  multi-cluster  framework 
(see  Figure  51)  which  will  allow  the  number  of  processors,  inter-processor 
communication,  and  UAV  communication  to  be  dynamically  optimized.  This  will  lead 
towards  a  new  generalization  of  scheduling  tradeoffs  between  perfomiance  and  resources 
using  control  metrics.  This  could  include  dynamic  varying  and  optimizing: 

•  The  number  of  computing  nodes  per  RHC  subsystem 

•  Computational  delays 

•  Communication  delays 

•  The  RHC  horizons 

•  Additional  RHC  control  design  parameters 

•  Computation  for  centralized  cooperative  control  schemes 

•  Computation  and  communication  for  low  level  coni 

2-  RHC  and  scheduling  algorithms  on  multi-rotorcraft  experimental  test-bed  (Task  13) 
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computational  partitioning  for  each  UAV 
(more  nodes  for  UAV  2) 


computational 

node 

Figure  51-  Multi-cluster  framework  for  multi-UAV  control  systems  at  the  CIS  lab 

(48  node  example) 
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5.7  APPENDIX  A:  Wireless  Inertiacube3  troubleshooting 

In  order  to  start  working  with  wireless  Inertiacube3,  the  following  steps  should  be  done: 

1.  Attach  the  Receiver  to  the  USB  port  of  computer  and  install  the  driver  and 
programs  as  described  in  [24], 

2.  Attach  a  6  to  9  V  battery  to  the  sensor^'^  and  run  “Device  Tool”.  Click  on  the 
yellow  bulb  on  top  left  of  the  screen  of  Device  Tool.  If  it  could  not  detect  the 
sensor,  from  the  “Wireless”  menu,  run  “search  for  stations”.  If  it  could  not  detect 
the  sensor  again,  restart  the  sensor  (just  detach  the  battery  and  attach  it  again)  and 
click  on  the  yellow  bulb.  If  the  problem  exists  do  the  previous  command 
(restarting  the  sensor  and  clicking  on  the  yellow  bulb)  for  2  to  3  times.  If  the 
sensor  docs  not  work,  consult  with  the  manufacturer. 

3.  When  the  sensor  was  detected,  run  ISDcmo  (with  dll  compatible  mode)  to  sec 
how  the  sensor  is  working. 

For  trouble  shooting  refer  to  [25].  The  most  popular  problems  and  probable  ways  to  fix 
them  are  listed  bellow: 


Problem 

Reason/Solution 

The  sensor  interference  or 
lack  of  accuracy 

Check  for  the  following  if  the  sensor  is  in  full  or  partial  compass 
mode: 

1)  Placement  directly  on  top  of  metal.  You  should  place  the 
wireless  lncrtiaCubc3  an  inch  or  two  away.  If  the  lnerliaCubc3 
must  be  mounted  in  a  metallic  environment,  follow  the  steps  in  [25] 
(Section  4.7)  to  perform  a  magnetic  calibration  using  the  “Compa.ss 
Calibration  Tool”. 

2)  A  varying  magnetic  field  may  exist  in  the  area.  Check  the  area 
with  a  compass  to  be  sure  about  the  exi.stence  of  the  varying 
magnetic  field.  In  this  case  the  best  thing  is  changing  the  working 
area. 

Orientation  is  drifting 
uncontrollably 

1)  Make  sure  that  the  sensor  is  properly  plugged  in. 

2)  Keep  the  sensor  still  for  10  seconds  after  connecting  with 

ISDEMO  or  DLL. 

3)  Compass  is  turned  off  in  ISDEMO. 

The  user  is  encouraged  to  sec  FAQ  [34]. 


For  specifications  of  battery  see  [24]. 
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5.8  APPENDIX  B:  RHC.h 


class  RHC  ( 

//  internal  variables  for  Powell's  optimization  functions 
int  ncom; 

double  "pcom,  *xicom; 

d'^/ector  *ptt  RHC,  *pt_RHC,  *xit  RHC,  'xt  RHC; 

public: 

double  tO;  //  initial  time 
double  t;  //  current  time 

double  tb;  //  time  that  interpolation  begins 

dVector  *ti;  //  interpolation  time 
dVector  'ts;  //  time  at  spline  points 

double  dti;  //  interpolation  time  interval 

double  T;  //  RHC  optimization  horizon 
double  delta;  II  RHC  execution  horizon 

int  Nx;  II  number  of  states 
int  Nu;  II  number  of  inputs 
int  Nz;  II  number  of  flat  outputs 
int  Ny;  II  number  of  outputs 

dVector  *X0;  //  initial  condition  state 

dVector  *U0;  //  initial  input 

dVector  *X;  //  current  state  (at  time  t) 

dVector  *U;  //  current  input 

dVector  *Y;  //  current  output 

dVector  *Z;  II  current  flat  output 

dVector  *Xd;  //  current  state  derivative 

int  Ns;  //  number  of  spline  control  points  for  each  output 

int  Ni;  //  number  of  spline  interpolation  points  for  each  output 

int  Nc;  //  number  of  varying  control  points 

int  Nd;  //  maximum  number  of  output  derivatives  required 

int  ks;  II  order  of  bspline 

dMatrix  *Cs;  //  spline  control  points  Cslqlljl  for  flat  output  q,  control  point  :) 
dVector  *Cv;  //  vector  containing  all  varying  control  points 

dMatrix  *Xi;  //  state  Xi[j)liJ  for  each  interpolation  point  i 

dMatrix  *Ui;  II  input  UiljJliJ  for  each  interpolation  point  i 

dMatrix  *Yi;  //  output  Yilj)[iJ  for  each  interpolation  point  i 

//  flat  output  q  (1  to  Nz)  for  each  derivative  id  (0  to  Nd) 

//  at  each  interpolation  time  i 
dMatrix  array  *2i;  II  ZilqllidJli) 

II  flat  output  value  at  the  beginning  of  the  interpolation  time  tO 
dMatrix  'ZiO;  II  ZiO[ql[idl 

II  flat  output  value  at  the  end  of  the  interpolation  time  tO+T 
dMatrix  *ZiT;  II  ZiT[q] lid) 

//  cost  function  parameter  matrices 
dMatrix  *Q;  //  (Nx  x  Nx) 
dMatrix  *R;  //  (Nu  x  Nu) 
dMatrix  *P;  //  (Nx  x  Nx) 
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*xd4 

*u2. 


//  Bs  -  bspline  basis  functions,  Bs[idlli)[jl  for  derivative  id  (0  to  Nd) , 

II  interpolation  time  i  (1  to  Ni),  control  point  j  (1  to  Ns) 
dMat rix_array  *Bs;  //  BslidlliJl]) 

II  second  derivative  at  control  points  C2iq)ljJ  for  flat  output  q, 

II  control  point  j  for  cubic  spline  interpolation  method 
dMatrix  ”C2; 

dMatrix  'vi; 

//  output  variables 

double  tout_final;  //  final  output  time 
int  Nout;  //  number  of  output  points 
int  Nsamp;  //  number  of  output  samples 

dMatrix  *Xout;  //  state  XoutljJ(i)  for  each  time  toutli] 

dMatrix  *Uout;  //  input  Uout(j]liJ  for  each  time  tout(i) 

dMatrix  ”Yout;  II  output  YoutljlU]  for  each  time  tout[i) 

dMatrix  'Zout;  //  flat  output  Zoutijlli)  for  each  time  tout[i] 

dVector  'tout;  II  output  time  with  spacing  at  the  interpolation  points  dti 

int  Imethod;  //  interpolation  method  [  1  =  cubic  spline,  2  =  bspline  J 

//  contructor  function 

RHC(double  to,  double  T,  double  delta,  int  Nx,  int  Nu,  int  Nz,  int  Ny, 

dVector  4x0,  dVector  4u0,  int  Ns,  int  Ni,  int  Nc,  int  Nd, 
double  tout_final,  int  Imethod,  int  ks); 

//  destructor  function 
'RHC  ( ) ; 

//  function  to  optimize  using  Powell's  method 
double  func_powell (dVector  4c); 

II  cost  function  for  trajectory  generation 
double  calc  J (dVector  4c) ; 

//  optimize  the  cost  function 
optimize  cost  ( ) ; 

interpolate  ( ) ; 

//  calculate  Xi  and  Ui  based  on  the  flat  outputs  Zi 
calculate  XU  ( ) ; 

//  calculate  the  input  at  time  tc 
calculate  U (double  tc) ; 

//  simulate  the  system  from  the  current  time  t  to  the  final  time  tf 
//  using  the  optimal  input  Ui  and  a  time  step  dt 

//  the  result  is  stored  in  the  output  variables  Xout,  Uout,  Yout,  tout 
simulate  interval  (double  tf,  double  dt); 

//  calculate  the  derivative  vector  xd  -  f(x,u,t)  of  the  actual  system 
calculate_xd (dVector  'x,  dVector  *u,  double  t,  dVector  'xd); 

calculate_outputs (dVector  *x,  dVector  'u,  double  t,  dVector  *y,  dVector  'z); 

//  save  variables  into  a  text  file 
save  data (char  'file  name) ; 

//  save  flat  output  variables  over  the  current 
II  interpolation  interval  into  a  text  file 
save  flat  outputs (char  'file  name) ; 

RHC; : compute_flat_ICs (dVector  4x0,  dVector  4u0,  dVector  4ud0); 

u_compute_C'!  (double  *t,  double  *x,  double  *xd,  double  *xd2,  double  *xd3, 
double  *y,  double  *yd,  double  *yd2,  double  'yd3,  double  *yd4,  double  ‘ul, 
double  'psi,  double  'psid,  int  ni); 

//  Powell's  method  optimization  functions 


double 

double 
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void  powell (dVector  sc,  double  **vi,  int  n,  double  ftol,  int  siter,  double 
void  linmin (double  pll,  double  xil),  int  n,  double  "fret); 

void  mnbrak (double  *ax,  double  *bx,  double  *cx,  double  ’fa,  double  ’fb. 


sf rot)  ; 
double 


fc)  ; 


double  brent (double  ax,  double  bx,  double  cx,  double  tol,  double  ’xrnin) ; 
double  fldim(double  x) ; 
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5.9  APPENDIX  C:  Program  heli  local.cpp 

•include  <cmath>  //  math  functions 
•include  <cstdio>  //  standard  I/O  functions 
•include  <cstring>  //  string  manipulation  functions 

•include  <iostream>  //  console  stream  I/O 
•include  <fstream>  //  file  stream  I/O 
•include  <strstream>  //  string  stream  I/O 

•  include  <conio.h>  //  console  I/O  functions  such  as  getchO 

using  namespace  std; 

•include  "nrutil.h" 

•include  "recipes. h" 

•include  "cubic_spline . h" 

•include  "timer. h" 

•include  "dVector.h" 

•include  "dMatrix.h" 

•include  "dMatrix  array. h" 

•include  "RHC.h" 

using  namespace  std; 

void  maint) 

( 

double  tO, T, del ta , t f , t ; 
in t  Nx,  Nu,  Nz ,  Ny ; 
int  Ns,  Ni,  Nc,  Nd; 
int  Imethod; 
int  ks; 

to  =  0.0; 

//  setting  key  dimensions,  refer  to  RHC.h 
Nx  =  6; 

Nu  =  2; 

N  z  =  2 ; 

Ny  =  3; 

Nd  =  2; 

dVector  x0(l,Nx),  uO(l,Nu); 

Ns  =  3+1; 

Ni  =  bO+1; 

T  =  1.0; 

delta  =0.1; 

//  number  of  free  optimization  parameters 
//  ***  this  one  needs  to  be  carefully  explained 
Nc  =  Nz*Ns  +  1; 

tf  =  2*3. 141 &9; 

Imethod  =  1;  //  cubic  spline  interpolation  method 
//  bspiine  order 

ks  =  6;  //  must  be  at  least  nd  +  1 
//  declare  two  RHC  objects 

RMC  rhcl(tO,T,delta,Nx,Nu,Nz,Ny,xO,uO,Ns,Ni,Nc,Nd,tf,Imethod,ks) 
//  set  ICs 

rhcl.X->e(l)  =  0.0;  //  u 
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rhcl.X->e(2)  «  0.0; 

// 

V 

rhcl.X->e(3)  =  0.0; 

// 

r 

rhcl . X->e  ( 4 )  =  0.0; 

// 

psi 

rhcl.X->e(5)  =  2.0; 

// 

xc 

rhcl.X->e(6)  -  0.0; 

// 

yc 

//  optimize  the  cost  function  over  one  horizon 
rhcl .optimize_cost () ; 

//  save  the  flat  outputs  and  related  states  for  this  horizon 

//  the  main  purpose  of  this  is  for  debugging  and 

//  understanding  how  good  one  optimization  horizon  is 

II  this  may  be  good  for  tuning  the  parameters 

II  also  good  for  checking  your  flat  output 

//  description  of  your  system 

rhcl . savef latoutputs ( "f . dat" ) ; 

II  the  first  two  calls  are  not  needed  to  simulate 
//  this  RHC  example 


II  interpolation  time 
fprintf ( fp, "%le  ", ti->e (i) )  ; 

//  interpolation  states 
for  ( j=l ; j<=Nx;  j  +  +  )  ( 

//  input  Xi [ j 1 [i) 

fprintf (fp, "%le  ",Xi->e(j, i) ) ; 

1 

//  inputs  corresponding  to  flat  outputs 
for  ( j-1;  jk’^Nu;  j  +  +)  ( 

/ !  input  Ui [ j 1 [ 1 ] 

fprintf (fp, "%le  ",Ui->e(j, i) ) ; 

I 

//  flat  outputs 
for ( j=l; j<=Nz; j++)  ( 

for  ( id=0;  id<=Nd;  ld+■^ )  ( 

//  Zi[ql Ild) [il 

fprintf  (fp, "tie  ", Zi->e (j, id, i) ) ; 

I 

1 


II  simulate  RUC  controller  for  3dof  helicopter  in  local  coordinates 
t  =  delta;  II  delta  =  execution  horizon  (the  sampling  rate) 
whilo(t  <  (tf+lo-7))  { 

rhcl . opt imize_cost 0 ;  //  optimize  RHC  over  T 

//  simulate  system  until  time  t  (t  <=  T) 

//  in  this  case  since  we  are  incrementing  t 
//  by  delta  (the  execution  horizon) , 

II  so  we  simulate  over  the  execution  horizon 

//  this  approximates  the  behavior  of  a  real  RHC  system 

//  when  we  apply  the  input  over  the  execution  horizon. 

//  the  user  can  specify  the  actual  model  that  is 
II  used  for  the  simulation  in  RHC  user  (calculate_xd) . 

II  this  can  be  useful  to  test  the  robustness  to  uncertainty 
II  the  dt  for  the  simulation  is  made  smaller  than  dti  ('r/(Ni-l)) 
It  to  get  a  more  accurate  simulation  using  Euler's  method 
rhcl. simulate  interval (t, rhcl.dti/1000) ; 

t  +=  delta; 

1 

II  save  the  simulation  output  into  a  file 

//  this  function  saves  the  output  data  at  time  intervals 
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//  of  dti  so  there  isn't  too  much  data 
rhcl . save_da ta ( "a .dat" ) ; 

for ( 1=1 ; i<=Nsamp; 1++ )  ( 

fprintftfp,  ’'%le  ",tout->e(i)  ); 

for  ( j  =  l ; j<=Nx; j +  + )  fprintf ( fp, "%le  " , Xout->e  ( j , i ) ) 
for  ( j=l ; j<=Nu; j  +  +  )  f print f  (  fp, "% le  " , Uout->e  ( j , i ) ) 
for ( j=l ; j<=Nz; j++)  fprintf ( fp, "%le  " , Zout->e ( j , i ) ) 
for  ( j  =  l ; j<=Ny; j++)  f printf  (  fp, "%le  " , Yout->e  ( ] , i ) ) 
fprintf  (fp, "in") ; 

) 


) 


printf ("\n\ndone. ") ; 
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5.10  APPENDIX  D:  RHC  user  heli  local.cpp 


#include  <cmath>  //  math  functions 
linclude  <cstdio>  //  standard  I/O  functions 
#inciude  <cstring>  //  string  manipulation  functions 

#include  <iostream>  //  console  stream  I/O 
♦include  <fstreara>  //  file  stream  I/O 
♦include  <strstream>  //  string  stream  1/0 


♦include  <conio.h>  //  console  I/O  functions  such  as  getch() 


using  namespace  std; 


"nrutii.h" 
"recipes . h" 
"cubicspiino . h" 
"timer . h" 


♦  include 
♦include 
♦include 
♦include 

♦include 

♦include 

♦include 

♦include 

♦include 


"dVector 
"dMatrix 
" dMa  t  r 1 X 

"RUC.h" 


■  h" 

■  h" 

_a  r ray .  h 


"hell  models. h" 


RHC calculate_U (double  tc) 

//  calculate  the  input  at  time  tc 
I 

int  1 ,  j  ,  i2 ; 

double  ul,u2,tl,u; 

const  double  TINY  =  l.Oe-7; 


//  approximate  method  using  linear  interpolation 
//  between  interpolation  points  ti 
1  -  (int)  (tc/dti  1-  TINY)  1; 
for ( j=l ; ]<=Nu; j +* )  ( 

ul  -  Ui->e(j,i); 

i2  -  1  t  1; 

if  (i2  >  Ni)  i2  =  Ni; 
u2  =  Ui->e  ( j , i2)  ; 
tl  =  (1  -  l)*dti; 
u  =  ul  +  (u2-ul) /dti* (tc  -  tl); 

U->e(j)  =  u; 

1 

) 


RHC : : calculate_xd  (dVector  "x,  dVector  *u,  double  t,  dVector  *xd) 
( 

//  use  local  3dof  heli  model 
f_heli_3dof2 (x->A, u->A, xd->A) ; 

) 


RHC: : calculate_outputs  (dVector  *x,  dVector  'u,  double  t,  dVector  *y, 
I 

//  u  =  XIU 
//  V  =  X12] 

//  r  =  X13J 
/ /  psi  =  X [ 4 ) 

//  xc  -  X[5] 

//  yc  =  X16) 

//  yl  =  xc,  y2  =  yc,  y3  =  psi 
y->e ( 1 )  =  x->e  (5) ; 
y->e  (2)  =  x->e  (6)  ; 
y->e (3)  -  x->e  (4)  ; 


dVector  *z) 
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//  zl  =  u,  z2  =  psi 
z->e  ( 1 )  =  x->e ( 1 ) ; 
z->e (2 )  =  x->e ( 4 ) ; 

) 

const  int  NMAX  =  5000; 

double  xcd [NMAX 1 , ycd [ NMAX ) , v [NMAX J , vd 1 NMAX ) ; 

RHC; icaiculate  XU  ( ) 

//  calculate  Xi  and  Ui  based  on  the  flat  outputs  Z\ 

1 

I  n  t  1 ,  q ; 
double  *ti; 
double  "u.'ud; 

double  *ul, 'u2, *psi, *psid, *psidd; 
double  c_psi , s_ps 1 , dt ,  V_inf ; 

double  S_fus_x, S_fus_y,m, Izz,l_tr,  k_mr  x, rho  a, cx, cy; 
double  suml,sum2; 

//  set  the  helecoper  paramters  with  the  values  from  the  report 
Sfusx  =  0.1; 

S_fus_y  =  0.22; 
m  =  8.2; 

Izz  =  0.28; 
l_tr  =  0.91; 
k  mr  X  =  -0.3; 
rhoa  =  1.0; 

cx  =  -0 . 5*rho_a*S_fus_x; 

//  cy  =  -0 . 5  * rho_a*S_fus_y; 

cy  =  0.0; 

//  this  part  is  not  so  profound 

II  is  because  we  have  two  names  for  ti 

II  the  one  inside  the  RHC  class  is  actually  a  dVector  pointer 
II  this  "ti"  IS  a  ID  array  of  doubles 
ti  =  this->ti->A; 

q  =  1; 

u  -  Zi->Mlq|->A[0) ; 

ud  =  Zi->Mlql->A[ll  ; 

q  =  2; 

psi  =  Zi->M [ q) ->A ( 0 ] ; 
psid  =  Zi->M I q) ->A 1 1 ] ; 
psidd  =  Zi->M Iql ->A 1 2 J ; 

ul  =  Ui->A[1]  ; 
u2  =  Ui->A12] ; 

//  compute  vdlij,  assuming  cy  =  0 
for ( i=l ; i<=Ni ; 1 ++ )  ( 

//  vd  -  -u*r  +  cy  *  V  *  \/_inf  /  m; 
vd[ij  -  -u 1 1 1 ’psid [ 1 ] ; 

} 

//  compute  v(il  using  trapezoidal  integration 
1  =  1; 

suml  =  v[i]  -  Xi->e(2,i)  =  X->e(2);  II  ICs 

dt  =  T/ (Ni-1) ; 

for ( i=2 ; i<=Ni ; i++ )  ( 

suml  +=  0.5*(  vdli-lj  +  vdli)  )*dt; 
v[i]  =  Xi->e(2.i)  =  suml; 

) 

II  compute  Ul 
for(i=l;i<=Ni;i++)  ( 

V_inf  -  sqrt (u 1 1 1 'u  1 1 )  +  vlil*v[i]); 

II  ud  =  Vr  -  k  mr  X  *  Tmr  /  m  +  cx  *  u  *  V  inf  /  m 
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)  ; 


ulUl  =  (  ud[il  -  vlil‘psid[il  -  cx  *  uli]  '  V_inf  /  m  )  /  ( 


//  rd  =  -Ttr*i_tr/Izz; 
u2[il  =  psidd [ 1 1 / (-l_t r/ 1  zz)  ; 

1 

//  calculate  Xi 

f or  ( 1=1 ; i<-Ni ;  1++ )  ( 

Xi->e(l,i)  =  u[il;  //  u  =  XllJ 
Xi->e(2,i)  =  v[i];  //  v  =  X[21 
Xi->e(3,i)  =  psid[i];  //  r  -  X13] 
Xi->e(4,i)  =  psilil;  //  psi  =  X [ 4 J 

1 


//  need  to  integrate  u,  v  to  get  xc,  yc 
for ( 1=1 ; i<=Ni ; i++ )  ( 

c_psi  =  cos(psi[il);  spsi  =  sin (psi 1 i 1 ) ; 

//  express  velocity  vl=lu,vj  in  global  coord  using  Vg  =  R'vl 
xcd(il  =  c_psi*u[i]  -  spsi'vli]; 
ycd  1 1 1  =  s_psi*u[il  +  c  psi'vli]; 

I 

//  trapezoidal  integration 
1  =  1; 

suml  =  Xi->e(5,i)  =  X->e(5);  II  ICs 
sum2  =  Xi->e(6,i)  =  X->e(6); 
dt  =  T/  (Ni-1)  ; 
f or ( 1=2 ; i<=Ni ; i++ )  ( 

suml  +=  0.5* (  xcd[i-l]  +  xcd[i]  ) *dt; 

Xi->e(5,i)  =  suml; 

sum2  +=  0.5*  (  ycd[i-lj  +  ycd[il  ) *dt; 

Xi->e  (6,1)  =  sura2 ; 

1 


double  RHC : : calc  J ( dVector  sc) 

//  cost  function  for  trajectory  generation 
//  c  is  very  important  —  all  free  control  points 
( 

int  i,j,q,id,lc; 
double  t0,tf,dt; 
double  sum; 
double  J, L; 

double  xl , x2 , x3, x4, x5, x6, ul , u2; 
double  w.  Ax,  Ay,  xr,  yr,  tr; 

w  =  1.0; 

Ax  =  2.0; 

Ay  =  1.0; 

//  the  first  control  points  are  equal  to  the  ICs 

1  =  1.- 

q  =  1; 

Cs->e(q,3)  =  X->e(l);  II  u 
q  =  2; 

Cs->e(q,j)  =  X->e(4);  //  psi 

1C  =  0;  //  Index  for  optimization  vector  c 

II  this  following  part  can  vary  depending 

II  on  how  we  want  to  arrange  our  free  control  points 

//  of  our  flat  outputs  Into  the  vector  c 

lie-  [u  free  psi  free] 

II  flat  output  #1  (u) 


-)r  mr  X  /  m 
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q  =  1; 

II  get  control  points  from  optimization  vector  c 
for ( j=2 ; j<=Ns; j++ )  { 

1  c  +  +  ; 

Cs->e(q,j)  =  c.e(ic); 

1 

//  define  boundary  conditions  of  the  spline 
id  =  l,• 
l  c  +  +  ; 


II  ZiO  -  boundary  condition  derivatives  (including  zero  derivative)  at  t=0 
ZiO->e(q,id)  =  c.e(ic);  II  end  point  derivative  is  free 


I  c  +  +  ; 

II  ZiT  -  boundary  condition  derivatives  (including  zero  derivative)  at  t-T 
ZiT->e(q,id)  =  c.e(ic);  II  end  point  derivative  is  free 

//  flat  output  02  (psi) 
q  =  2; 

II  get  control  points  from  optimization  vector  c 
for ( j=2 ; j<=Ns; j ++)  ( 
i  c  +  + ; 

Cs->e(q,j)  “  c.e(ic); 

I 


II  define  boundary  conditions  of  the  spline 
Id  =  1; 

II  psid  =  r  =  X[3] 

ZiO->e(q,id)  =  X->e(3);  //  fixed  by  IC 

I  c  +  + ; 

ZiT->e(q,id)  =  c.e(ic);  II  end  point  derivative  is  free 
II  printf("\nic  =  %d",ic); 

II  spline  interpolation  !  II II II II II II II II II II ! 
interpolate () ;  II  RUCOOL  does  this 

//  calculate  Xi,Ui  based  on  Zi  II II II II II II II ! 
caiculate_XU ( ) ;  //  you  programed  this  in  RHC_user.cpp 

//  integrate  cost  function  II II II II II II II II II ! 

II  initial  and  final  time 

to  =  t; 

tf  =  t»T; 

II  calculate  the  cost  function 

//  J  =  1/2  •  int  (xl'Z  +  x2''2  +  u'2) 

//  using  trapezoidal  integration 

//  I  =  dt/2  '  (f(tO)  +  2'f(tl)  +  ...  +  2*f(tn-l)  +  f(tn)) 

dt  =  (tf-tO) /  (Ni-1) ; 

sum  =  0.0; 

for  ( i=2 ; i<Ni ; i +  + )  ( 


xl 

= 

Xi->e  ( 1 , i ) ; 

x2 

= 

Xi->e (2, i ) ; 

x3 

= 

Xi->e  (3/ i) ; 

x4 

Xi->e  (4, i ) ; 

x5 

Xi->e  (5, i) ; 

x6 

= 

Xi->e (6, 1 ) ; 

ul 

= 

Ui->e  ( 1 , i ) ; 

u2 

= 

Ui->e (2, i) ; 

tr 

tb  +  ti->e  (  i ) ; 

xr 

= 

Ax*cos {tr) ; 

yt 

= 

Ay*sin  (w*tr) ; 

xr 

= 

tr ; 
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// 

// 


// 

// 


// 

I 


yr  =  xr  *xr ; 

L  =  O'xl'xl  +  0*x2*x2  +  0*x3*x3  +  0*x4'x‘1  +  (x5-xr)  *  (x5-xr) 
^  (x6-yr) * (x6-yr)  »  0.0*ul*ul  +  0.0*u2*u2; 

sum  +=  2.0*L; 

I 

//  add  the  endpoints 
i  =  1; 

xl  =  Xi->e  (1,1); 
x2  =  Xi->e  (2,1)  ; 
x3  =  Xi->e (3,1); 
x4  =  Xl ->e  (4,1); 
x5  =  Xi->e  (5,1)  ; 
x6  =  Xi->e  (6,1); 
ul  =  Ui->e(l,i); 
u2  =  Ul ->e  (2,1); 

tr  =  tb  +  ti->e(i); 
xr  =  Ax'cos  (tr) ; 
yr  =  Ay*sin(w'tr); 

X  r  =  t  r ; 
yr  =  xr*xr; 

L  =  O’xl'xl  +  0*x2'x2  +  0'x3'x3  +  0'x4*x4  +  ( xb-xr ) * (x5-x r ) 

+  (x6-yr) * (x6-yr)  +  0.0'ul'ul  +  0.0'u2*u2; 
sum  +=  L; 

1  =  N 1  ; 

xl  =  Xl ->e  ( 1 ,  i )  ; 
x2  =  Xi->e  (2, i )  ; 
x3  =  Xi->e  (3,  i)  ; 
x4  =  Xi->e  (4, i)  ; 
xb  ^  Xi->e  (b,  i)  ; 
x6  =  Xi->e (6, i)  ; 
ul  =  Ui->e(l,i); 
u2  =  Ui->e  (2, i )  ; 

tr  =  tb  +  ti->e  (i) ; 
xr  =  Ax'cos ( t r) ; 
yr  =  Ay'sin  (w'tr) ; 

X  r  =  t  r  ; 
yr  =  xr'xr; 

L  =  O'xl'xl  *  0'x2'x2  '  0'x3'x3  +  0'x4*x4  +  ( xb-xr ) • ( xb-xr ) 

+  (x6-yr) '  (x6-yr)  +  0.0'ul'ul  +  0.0*u2'u2; 

sum  L; 

//  calculate  inetgral 
sum  =  dt/2.0'sum; 

//  calculate  cost  function 
J  =  O.b'sum; 

printf (" \nj (debug)  =  %lf",J); 
return  J; 


RHC :: compute  f iat_ ICs (dVector  SxO,  dVector  SuO,  dVector  SudO) 
( 


1 


void  f_heli_3dof2 (double  'X,  double  'U,  double  'Xd) 

//  hell  model  in  local  coord  with  combined  expressions 
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//  X[l]  -  u  (velocity  along  body  x  axis) 

//  X[2)  -  V  (velocity  along  body  y  axis) 

//  X[31  -  r  (angular  velocity  along  body  z  axis) 

//  X[41  -  psi  (yaw  euler  angle  -  z  axis) 

//  X[51  -  xc  (center  of  mass  position  in  global  coordinates) 

//  X16]  -  yc  (center  of  mass  position  in  global  coordinates) 

( 

double  u,v,ud,vd; 
double  r,rd; 
double  psi,psid; 
double  xc, yc, xcd, ycd; 

double  c_psi;  //  cos  of  angles 
double  spsi;  //  sin  of  angles 

//  inputs 

double  u_mr;  //  main  rotor 

double  u_tr;  //  tail  rotor 

//  parameters 
double  m, Izz ; 
double  Itr; 

double  )<_mr_x;  //  gain  from  main  rotor  thrust  to  x  thrust  component 
double  Sfusx, S_fus_y; 
double  rho_a;  //  air  density 

double  Tmr;  //  main  rotor  thrust 
double  Ttr;  //  tail  rotor  thrust 

double  V_inf; 
double  cx,cy; 

//  set  the  helecoper  paramters  with  the  values  from  the  report 
Sfusx  =  0.1; 

Sfusy  =  0.22; 
m  =  8.2; 

Izz  =  0.28; 
l_tr  =  0.91; 
lc_mr_x  =  -0.3; 
rhoa  =  1.0; 

cx  =  -0 . 5*rho_a*S_fus_x; 

//  cy  =  -0 . 5*rho_a*S_fus_y; 

cy  =  0.0; 

//  get  state  variables 
u  =  XU); 

V  =  X[2]; 
r  =  X[3); 
psi  =  X  I  4 ] ; 
xc  »  x I  5  J ; 
yc  =  X  (  6  ]  ; 

//  get  inputs 
//  U  =  lu_mr,  u_tr) 
u_mr  -  0(1); 
u  tr  =  U12J ; 

//  assume  the  main  rotor  and  tail  rotor  thrust  are  commanded  by  the  inputs 
Tmr  =  u_mr; 

Ttr  =-  u_tr; 

V_inf  =  sqrt(u*u  +  v*v); 

II  flat  outputs  (zl,z2)  =  (u,psi) 

ud  =  v*r  -  k_mr_x  ’  Tmr  /m+cx*u*V  inf  /  m; 

//  for  this  equation  we  can  integrate  it  using  Kuler  or  RK 
//  however  ODE  integration  is  time  consuming  and  not 
II  accutate  compared  to  integration,  so  we  assume  cy  =  0 
vd  =  -u*r  +  cy  *  V  *  V_inf  /  m; 
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rd  =  -Ttr*l_tr/lz2; 

//  euier  angle  derivative 
psid  =  r; 

//  calculate  some  trig  functions 
c_psi  =  cos(psi);  spsi  =  sin(psi); 

//  global  position  derivatives 

//  express  velocity  vl=[u,vj  in  global  coord  using  Vg  =  R*vl 
xcd  =  c_psi*u  -  s_psi*v; 
ycd  =  s_psi*u  +  c_psi*v; 

Xd[l)  =  ud; 

Xd[2)  =  vd; 

Xd[3]  =  rd; 

Xd[4]  =  psid; 

Xd[51  =  xcd; 

Xd[61  =  ycd; 
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5.11  APPENDIX  E:  dVector.cpp 


linclude  <cmath>  // 
ttinclude  <cstdio>  // 
tinclude  <cstring>  // 


math  functions 
standard  I/O  functions 
string  manipulation  functions 


//  for  real-time  program  remove  some  headers 
#ifndef  RTSS 


linclude  <iostream>  //  console  stream  1/0 
linclude  <fstream>  //  file  stream  I/O 
tlinclude  <strstream>  //  string  stream  I/O 

(•include  <conio.h>  //  console  1/0  functions  such  as  getchl) 
using  namespace  std; 

(tendif 


#include  "dVector.h" 

dVector : : dVector  ( int  nl,  int  n2) 

( 

int  1 ; 

safe_reference  =  0.0; 

//  initialize  the  member  variables  nl  and  n2 
N 1  =  n  1 ; 

N2  =  n2; 

//  initialize  dynamic  array  pointer 
allocate  vector (A, Nl , N2 )  ; 

lf(A==NULL)  ( 

(tlfdef  DEBUG 

cout  <<  "\nallocate  vector  error  in  vector :: vector  ( int  nl,  int  n 
#endi f 


return; 

) 

//  initialize  array  elements 
f or ( i=Nl ; i<=N2 ; 1++)  Ali]  =  (double)O.O; 

) 

dVector : : -dVector  ( ) 

( 

free_vector {A,  Nl ,  N2)  ; 

I 


int  allocatevector (double  ‘Sa,  int  nl,  int  n2) 

( 

//  check  for  invalid  arguments 
if (n2  <  nl)  ( 

tlfdef  DEBUG 

cout  <<  "\nerror  in  allocate  vector:  n2  <  nl"; 
tendif 
a  =  NULL; 
return  1; 

) 

//  allocate  a  dynamic  ID  array  of  doubles 
a  =  new  double  1  n2  -  nl  +  1  ] ; 

If (a  ==  NULL)  ( 

tlfdef  DEBUG 

cout  <<  "inmemory  allocation  error  in  al locatevector " ; 
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#endif 
return  1; 

I 

a  =  a  -  n  1  ; 
return  0; 

I 


int  freevector (double  *&a,  int  nl,  int  n2) 

( 

if (a  ==  NULL)  ( 

#ifdef  DEBUG 

cout  <<  "\nNULL  pointer  in  f ree_vector " ; 

Itendif 

return  1 ; 


a  =  a  +  n  1  ; 
delete  a; 
a  =  NULL; 
return  0; 

1 
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5.12  APPENDIX  F:  dMatrix.cpp 


#include  <cmath>  // 
iinclude  <cstdio>  // 
((include  <cstring>  // 


math  functions 
standard  I/O  functions 
string  manipulation  functions 


//  for  real-time  program  remove  some  headers 
Itifndef  RTSS 


♦include  <iostream>  //  console  stream  I/O 
((include  <fstream>  //  file  stream  I/O 
((include  <strstream>  //  string  stream  I/O 

((include  <conio.h>  //  console  I/O  functions  such  as  getchO 
using  namespace  std; 

♦endi f 


♦include  "recipes. h" 

♦include  "dVector.h" 

♦include  "dMatrix.h" 

dMa t r IX ; : dMa t r IX ( in t  nl,  int  n2,  int  ml,  int  m2) 

( 

int  1 , j ; 

safe_reference  =  0.0; 

Nl  =  nl;  N2  =  n2;  Ml  =  ml;  M2  =  m2; 

al locate_mat  rix ( A, Nl , N2 , Ml , M2 ) ; 

if(A==NULL)  ( 

♦ifdef  DEBUG 

cout  <<  "\nallocate_matrix  error  in  dMat rix : : dMa trix  ( int  nl,  int  n2, 

ml ,  int  m2 ) " ; 

♦endi f 
return; 

) 

//  initialize  the  array  elements 
for ( 1=N1 ; i<=N2 ; i++)  { 

f or ( j=Ml ; j <=M2; j ++)  ( 

Atilljl  =  (double)O.O; 


) 


} 


dMat rix : : -dMat rix  ( ) 

( 

f  ree  ma t  r IX  t A,  Nl ,  N2,  Ml ,  M2 )  ; 

) 


int  allocate_matrix (double  **sa,  int  nl,  int  n2,  int  ml,  int  m2) 
//  allocate  a  dynamic  2D  array 
I 

int  i,j; 


//  chec)^  for  invalid  arguments 
1 f ( n2  <  nl )  ( 

♦ifdef  DEBUG 

cout  <<  "\nerror  in  a  1 locate  dMat r ix :  n2  <  nl"; 
♦endif 
a  =  NULL; 


int 
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return  1 ; 

I 

//  check  for  invalid  arguments 
1 f (m2  <  ml )  ( 

#ifdef  DEBUG 

cout  <<  "Vnerror  in  allocate_dMatrix :  m2  <  ml"; 

#endif 
a  =  NULL; 
return  1; 

1 

//  allocate  a  dynamic  array  of  pointers 
a  =  new  double  *  ln2-nl+ll; 

//  check  for  memory  allocation  error 
if(a==NULL)  ( 

#ifdef  DEBUG 

cout  «  "\nmeraory  allocation  error  #1  in  al locatemat ri x" ; 

#endif 
return  1; 

I 

a  =  a  -  nl;  II  make  the  array  go  from  nl  to  n2 

//  initialize  each  pointer  in  the  array  a  to  a  ID  array  that  stores  each  row 
for ( i=nl ; i<=n2; i++)  ( 

all]  =  new  double  lm2-ml+l];  //  declare  a  ID  array  for  each  row 

//  if  there  is  an  allocation  error  we  need  to  free  memory  and  return  NULL 
if  (a[ij  ==  NULL)  ( 

for ( j=nl ; j<i ; j ++ )  delete  (al^J+ml);  II  delete  each  row  up  to  i-l 
delete  (a+nl);  //  delete  the  array  of  pointers 
#ifdef  DEBUG 

cout  <<  "\nmemory  allocation  error  #2  in  ailocate_dMatrix" ; 

(tendif 

a  =  NULL; 
return  1; 

1 

a[i]  =  ali)  -  ml;  //  adjust  for  the  column  offset  ml 

I 

return  0; 

1 


int  freematrix (double  **Sa,  int  nl,  int  n2,  int  ml,  int  m2) 

//  free  a  dynamic  2D  array  of  doubles 

I 

int  1  ; 

//  check  if  a  is  a  NULL  pointer 
if(a==NULL)  ( 

#ifdef  DEBUG 

cout  <<  "\nerror:  NULL  pointer  in  f ree_ma t r ix" ; 
#endif 

return  1;  II  error 

) 

f or ( i-nl ; i<=n2 ;  i++ )  delete  (ati)+ml);  //  delete  each  row 
delete  (a+nl);  II  delete  the  array  of  pointers 
a  =  NULL;  //  sot  the  pointer  equal  to  NULL 
return  0;  II  OK 
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1 


void  addtdMatrix  SA,  dMatrix  4B,  dMatrix  SC) 
//  C  =  A  +  B 


int  i,j; 

for(i=A.Nl,-  1<=A.N2;  i  +  +)  ( 

for{j=A.Ml;  j<=A.M2;  j++)  ( 

C.e(i,j)  =  A.e(i,j)  +  B.e(i,j); 

1 


) 


void  add(dVector  sa,  dVector  sb,  dVector  Sc) 

//  c  =  a  t  b 

( 

int  1  ; 

for(i»a.Nl;  i<=a.N2;  i++)  c.e(i)  =  a.e(i)  +  b.e(i); 

I 


void  sub(dMatrix  SA,  dMatrix  SB,  dMatrix  SC) 

//  C  =  A  -  B 

( 

int  1 , j ; 

for(i=A.Nl;  1<=A.N2;  i++)  ( 

for(j=A.Ml;  j<=A.M2;  j++)  ( 

C.e(i,j)  =  A.e(i,j)  -  B.e(i,j); 

1 

1 

1 


void  subldVector  Sa,  dVector  Sb,  dVector  Sc) 

//  c  =  a  -  b 

1 

int  i ; 

for(i-a.Nl;  i<=a.N2;  i++)  c.e(i)  =  a.o(i)  -  b.e(i); 
I 


void  mult (dMatrix  SA,  dMatrix  SB,  dMatrix  SC) 

//  multiply  two  matrices 
//  C  =  A  *  B 
( 

int  1,'i) ,  k; 

for(i=A.Nl;  i<=A.N2;  i++)  ( 

for(j=B.Ml;  j<=B.M2;  j++)  ( 

C.e(i,])  =  (double)O.O; 
for(k=A.Ml;  k<=A.M2;  k++)  ( 

C.e(i,j)  +=  A.e(i,k)  *  B.e(k,j); 


1 


) 


I 


void  mult  (dMatrix  SA,  dVector  sb,  dVector  Sc) 
//  multiply  a  matrix  times  a  vector 
//  c  =  A  •  b 

I 

int  1 , j ; 

for(i=A.Nl;  i<«A.N2;  i++)  ( 
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c.e(i)  =  (double)O.O; 
f  or  (  j-=A  .  Ml ;  j<=A.M2;  j+  +  ) 
c.e(i)  +=  A.e(i,j) 


) 


1 


1 


b .  e  ( ]  )  ; 


void  cross  (dVector  Sa,  dVector  4b,  dVector  4c) 
//  c  =  a  X  b 
//  all  vectors  are  1x3 
t 

c.xO  =  a.yO  *  b.z()  -  b.yO  ‘  a.z(); 

c.yO  =  b.x()  *  a.z()  -  a.xl)  *  b.z(); 

c.zO  =  a.xO  •  b.yO  -  b.xO  •  a.yO; 

1 


void  matrix_cross (dVector  4v,  dMatrix  4V) 

//  matrix  representation  of  cross  product 
//  V  -  input  vector  (1x3) 

//  V  -  matrix  representation  of  cross  product  (3x3) 

( 


V.e (1, 1) 

= 

(double) 0.0; 

V.e (1, 2) 

-v.zO; 

V.e (1, 3) 

= 

V  .  y  ( )  ; 

V.e  (2, 1) 

v  .  z  ( )  ; 

V.e (2, 2) 

= 

(double) 0.0; 

V.e (2, 3) 

= 

-v.x()  ; 

V.e(3,l) 

-v.y  ()  ; 

V.e(3,2) 

= 

v .  X  ( )  ; 

V.e (3, 3) 

(double) 0.0; 

void  transpose  (dMat r IX  4A,  dMatrix  4B) 

//  B  -  A' 

( 

1  n  t  1 ,  j  ; 

for(i=A.Nl;  i<=A.N2;  i++)  ( 

for(j=A.Ml;  j<=A.M2;  j++)  ( 

B.e(i,j)  =  A.e(j,i); 


1 

void  ludemp (dMatrix  4A,  dMatrix  &A_lud,  int  *indx) 
II  Calculate  the  LU  decomposition  of  a  nxn  matrix  a 
II  A_lud [ 1 . . n ] [ 1 . . n)  -  LU  decomposition  of  matrix  A 
//  indxIn+1)  =  -  row  permutation  index 
//  It  assumes  1  offset  arrays. 

( 

1  n  t  1 ,  j  ; 
double  *d,x; 

d  =  4  X ; 

//  A_lud  =  A 

for ( 1  =  1 ; i<=A_lud. N2;  1  +  +  )  ( 

for  ( j“l ; 3<=A_lud. M2 ;  j +  +  )  ( 

A_lud.A[il Ij)  =  A.A[il[jl; 

1 

) 

ludemp (A_lud . A,  A.N2,  indx,  d)  ; 

) 
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void  lu_solve (dMatrix  4A_lud,  int  *indx,  dVector  sb,  dVoctor  Sx) 
//  Return  the  solution  vector  x  of  Ax=b  using  the 
//  LU  decomposition  of  a  matrix. 

//  It  assumes  1  offset  arrays. 

( 

1  n  t  1  ; 

//  X  =  b 

f or t i=x . N1 ; i<=x . N2 ; i ++ )  x.Ali]  =  b.A[il; 
lubksb ( A_lud. A,  A_lud.N2,  indx,  x.A); 

) 


void  svdemp (dMatrix  sa,  dMatrix  4u,  dVector  Sw,  dMatrix  Sv) 

II  Calculate  the  Singular  Value  Decomposition 
II  of  a  matrix  A  =  UWV  . 

II  a  -  the  mxn  matrix  A  u  -  the  mxn  matrix  U 

II  V  -  the  nxn  matrix  V  w  -  the  n  vector  W 

( 

int  i , j ; 

II  u  =  a 

for(i=l;i<^a.N2;i++) 

for ( j=l ; j<=a .M2; j++)  u.e(i,3)  =  a.e(i,j); 
svdcmp(u.A,  a.N2,  a. M2,  w.A,  v.A); 

1 

void  svsoive (dMatrix  Sa,  dVector  Sb,  dMatrix  Su,  dVector  Sw,  dMatrix  Sv,  dVector  Sx) 
II  Return  the  solution  vector  of  Ax=b  using  the  SV 
//  decomposition  of  a  matrix. 

II  a  -  the  mxn  matrix  A  u  -  the  mxn  matrix  U 

II  V  -  the  nxn  matrix  V  w  -  the  n  vector  W 

II  X  -  the  n  solution  vector 

( 

int  1  ; 

double  tol=1.0e-5,  wmax»0.0,  thresh; 

//  threshold  singular  values 

for ( 1=1 ; i<=a .M2; i  +  +  )  i f  (w . e ( i ) >wmax)  wmax  =  w.e(i); 
thresh  =  tol*wmax; 

for ( i  =  l ; i<=a . M2 ; i +  + )  i f  (w . e ( i ) <thresh)  w.e(i)=0.0; 

svbksb(u.A,  w.A,  v.A,  a.N2,  a. M2,  b.A,  x.A); 

I 

double  cond (dMatrix  Sa) 

//  Find  the  condition  number  of  a  matrix  using  sv  demp 

( 

int  i ,  j  ; 

double  wmax,  wmin; 
dVector  w(l,a.M2); 

dMatrix  u(l,a.N2,  l,a.M2)  ,  v  ( 1 ,  a  .  M2 ,  1 ,  a  .  M2 )  ; 

//  u  =  a 

for ( 1=1 ; i<=a . N2 ;  i  +  + ) 

for ( j=l ; j <=a . M2 ; j +  + )  u.e(i,j)  =  a.e(i,j); 
svdcmp(u.A,  a.N2,  a. M2,  w.A,  v.A); 
wmin  =  wmax  =  w.e(l); 

f or ( 1=2 ; i<=a . M2 ; 1 + + )  if ( w . e ( i ) >wmax)  wmax  =  w.e(i); 
f or ( 1=2 ; i<=a . M2 ; 1  +  + )  if ( w . e  (i ) <wmin )  wmin  =  w.e(i); 


I 


return  (wmax/wmin) ; 
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5.13  G:  dMatrix  array.cpp 

Kinclude  <cmath>  //  math  functions 
Kinclude  <cstdio>  //  standard  I/O  functions 
Kinciude  <cstring>  //  string  manipulation  functions 

//  for  real-time  program  remove  some  headers 
(tifndef  RTSS 

#include  <iostream>  //  console  stream  I/O 
#include  <fstream>  //  file  stream  I/O 
#include  <strstream>  //  string  stream  I/O 

((include  <conio.h>  //  console  I/O  functions  such  as  getchO 
using  namespace  std; 

#endi f 

#include  "recipes. h" 

((include  "dVector.h" 

#include  "dMatrix.h" 

#include  "dMatrix_array . h" 

dMatrix  safe  dmatrix0(l,3,l,3); 

dMat r ix_array ; : dMat rixar ray ( int  kl,  int  k2,  int  nl,  int  n2,  int  ml,  int  m2) 

I 

int  k  ; 

safedMatrix  =  Ssafe_dmatrixO; 
safe  reference  =  0.0; 


Kl  = 

kl; 

K2  = 

k2; 

Nl  = 

nl  ; 

N2  = 

n2  ; 

Ml  = 

ml  ; 

M2  = 

m2  ; 

M  = 

new 

dMatr: 

LX  • 

if  (M 

== 

NULL) 

{ 

(tifdef  DEBUG 

cout  <<  "\nmemory  allocation  error  in  dMat rix_ array" ; 

#endi f 
return; 

) 

M  =  M-kl; 

for (k=Kl; k<=K2; k++)  Mlk]  =  NULL; 

for (k=Kl; k<-K2; k++)  ( 

MlkJ  =  new  dMa t r ix (nl , n2 , ml , m2 ) ; 
if (M[kJ  ==  NULL)  ( 

((ifdef  DEBUG 

cout  <<  "\nmemory  allocation  error  in  dMatrix  array"; 

#endif 

return; 

) 

) 

I 


dMatrix  array:  :~dMatrix  arrayO 

1 

int  k ; 

if  (.M  ==  NULL)  return; 
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for (k=Kl; k<=K2;k++)  ( 

if (  Mlk)  !=  NULL 

1 

M  =  M  +  Kl; 
delete  M; 


delete  M  1  k  J  ; 
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5.14  Appendix  H:  Explanation  of  Attachments 

In  this  chapter,  the  experimental  results  and  the  codes  written  for  the  materials  of  this 
report  which  arc  included  in  the  attached  CD  is  described.  The  data  is  classified  based  on 
the  different  chapters  of  this  report. 


Chapter  3 

All  of  the  data  used  for  chapter  3  of  the  report  arc  included  in  this  folder.  It  includes 
different  subfolders  based  on  the  report  and  the  contents  of  each  folder  arc  described 
bellow. 


Section  3.2 

All  of  the  supporting  data  for  section  3.2  of  the  report  is  included  in  this  folder.  It  has  the 
following  subfolders: 


3.2.1:  The  data  used  for  the  Figures  10  to  17  arc  collected  in  this  folder  with  name 
“3.2.1”.  This  folder  is  consists  of  three  parts  with  the  following  names: 
o  Part  one:  The  data  used  in  section  3. 2. 1.1  of  the  report  is  in  this  folder.  It 
consists  of  four  folders  named  “datal”,  “data4”,  “data?”,  and  “data9”.  Each  of 
them  has  a  C++  code  for  processing  data  to  identify  the  model  parameters  of 
Hovercraft  in  the  Forward  Movement.  By  running  this  code  the  values  b\, 


and  02  of  equation  (28)  arc  calculated  using  Singular  Value  Decomposition 
(SVD).  For  plotting  the  results  “plot  tlg.m”  should  be  run  using  MATLAB 
6.5^'. 

o  Part  Two:  The  data  used  in  section  3. 2. 1.2  of  the  report  is  in  this  folder.  It 
consists  of  one  folder  named  “data2”  which  has  a  C++  code  for  processing 
data  to  identify  the  model  parameters  of  Hovercraft  in  the  Rotation  Direction. 
By  running  this  code  the  values  b\,  a^  and  a^  of  equation  (30)  is  calculated 


using  SVD.  For  plotting  the  results  “plol  fig.m”  should  be  run, 
o  Part  Three:  The  data  used  in  section  3. 2. 1.3  of  the  report  is  in  this  folder.  It 
contains  two  folders  named  “Forward  Movement”  and  “Side  Movement”. 

■  Forward  Movement  has  two  folders  “dataS”  and  “dataS”.  Each  of  them 
has  a  C++  code  for  processing  data  to  identify  b[  of  equation  (28) 
using  SVD.  For  plotting  the  results  “plot_fig.m”  should  be  run. 

■  Side  Movement  has  one  folder  named  “data2”  which  contains  a  C++ 
code  for  processing  data  to  identify  of  equation  (29)  using  SVD. 
For  plotting  the  results  “plot_fig.m”  should  be  run. 


•  3.2.2:  In  this  folder,  all  of  the  results  used  in  the  section  3.2.2  of  the  report  are 

presented.  It  has  the  same  folders  as  previous  folder  (3.2. 1 )  but  all  of  the  data  used 


All  of  the  *.m  files  in  this  report  should  be  run  by  MATLAB  6.5. 
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to  find  the  model  parameters  are  included,  so  the  main  folders  are  the  same  as 
folder  3.2.1  but  these  folders  include  more  data. 

Section  3.3 

For  this  folder  “RHCOOL  Version  2”  is  included.  It  solves  the  example  of  3DOF 
helicopter  and  simulates  it.  The  path  followed  by  the  3DOF  helicopter  in  this  example  is 
an  ellipse. 


Chapter  4 

This  folder  includes  some  subfolders  which  are  described  bellow  and  each  of  the 
subfolders  has  one  “readme.doc”  file  which  describes  how  the  data  is  stored  in  all  of  the 
data  files  of  that  subfolder.  If  somebody  wants  to  do  more  process  on  the  data  file  of  that 
subfolder,  hc/she  should  refer  to  that  readme.doc  file. 

Section  4.1 

This  folder  includes  one  readme  file  described  before  and  one  folder  named  “4. 1. 1. 2”. 
The  experimental  data  of  Wireless  InertiaCube3  explained  in  section  4. 1. 1. 2  of  the  report, 
is  included  in  that  folder.  It  has  all  data  files  and  by  running  “plot_all_data.m”  Figure  28, 
Figure  30,  Figure  32,  and  Figure  33  of  the  report  will  be  regenerated. 

Section  4.2 

This  folder  includes  one  readme  file  described  before  and  two  folders  named  “4.2.1”  and 
“4.2.2”  as  follows: 

•  4.2.1:  By  running  “plot_f_m.m”  the  Figure  39  which  is  related  to  the  Forward 
movement  of  hovercraft  is  produced.  The  original  data  were  measured  by  PClBird 
[23]. 

•  4.2.2:  By  running  “plot_rotation.m”  the  Figure  40  which  is  related  to  the 
Rotational  movement  of  hovercraft  is  produced.  The  data  in  this  part,  measured 
by  wireless  lnertiaCubc3. 

Section  4.3 

This  folder  includes  one  readme  file  described  before  and  two  folders  named  “4.3.1”  and 
“4.3.2”  as  follows: 

•  4.3.1 :  this  folder  includes  tw'o  folders: 

o  Regulation  with  RHC:  the  original  data  of  Figure  42  of  the  report  is  in  this 
folder.  In  addition  by  running  “plot_data.m”  Figure  42  can  be  regenerated, 
o  Tracking  with  RHC:  the  original  data  of  Figure  43  of  the  report  and  a  Clip 
showing  the  hovercraft  while  tracking  a  sinusoidal  path  arc  in  this  folder. 
In  addition  by  running  “plot_data.m”  Figure  43  can  be  regenerated. 

•  4.3.2:  this  folder  includes  two  folders  as  bellow: 
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o  Vision_lnertiaciibe3:  this  folder  includes  the  original  data  of  Figure  44. 
This  results  show  the  tracking  of  one  hovercraft  while  the  orientation  of 
hovercraft  ( )  is  measured  by  vision  system  and  the  angular  velocity  ( if/  ) 
is  measures  by  wireless  InertiaCube3.  Figure  44  can  be  regenerated  by 
running  “plot_data.m”. 

o  only_vision:  the  original  data  of  Figures  45  and  46  arc  in  this  folder.  In 
this  case  all  of  the  data  required  for  controller  arc  measured  by  vision 
system.  These  two  figures  can  be  regenerated  by  running  “plot_data.m”. 

Section  4.4 

This  folder  includes  three  folders  as  bellow: 

•  Scheduling_two_systems;  this  folder  includes  one  readme  file  which  describes 
how  the  data  is  stored  in  “data.txt”.  By  running  “plot_data.m”  the  Figure  48  can 
be  regenerated.  This  folder  also  includes  another  folder  named 
“schcduling_vidco_clips”  which  contains  three  folders  named  “Video  1”, 
“Vidco2”,  and  “Video3”.  In  each  of  them  one  video  clip  of  scheduling  two 
hovcrcrafts  in  one  computer,  with  the  data  file  and  “plot_data.m”  are  exist. 

•  System_l;  this  folder  includes  one  readme  file  which  describes  how  the  data  is 
stored  in  “data.txt”.  By  running  “plot_data.m”  the  Figure  49  can  be  regenerated. 

•  System_2:  this  folder  includes  one  readme  file  which  describes  how  the  data  is 
stored  in  “data.txt”.  By  running  “plot  data.m”  the  Figure  50  can  be  regenerated. 

Chapter  6 

In  this  folder,  some  of  the  references  are  included  for  more  convenience  of  the  reader. 
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Abstract 

This  report  documents  the  work  related  to  task  #10  for  the  project  entitled  “Synthesis 
and  Implementation  of  Single  -  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear 
Control  and  Optimization  Techniques”.  Task#10  involves  theoretical  verification  of  the 
feasibility  and  stability  of  the  decentralized  receding  horizon  control  (RHC)  for  multi 
vehicle  systems.  The  stability  and  performance  of  decentralized  RHC  is  often  improved 
by  modifying  the  cost  function  and  constraints.  However,  variable  communication 
presents  an  additional  approach  for  further  improvement.  In  this  report,  using  variable 
communication  a  new  decentralized  RHC  algorithm  is  developed.  In  spite  of  the  other 
decentralized  RHC  methods  that  try  to  reduce  the  communication  requirements,  the 
proposed  method  allows  utilizing  the  maximum  available  communication  capacity  for 
further  improvement  of  the  stability  and  performance.  Through  modification  of  stability 
conditions  developed  for  time  varying  decentralized  RHC,  an  investigation  of  the  effect 
of  1-  faster  communication  rates,  2-  expanded  communication  radius,  and  3-  multi¬ 
hopping  communication  on  decentralized  RHC  is  performed.  Then,  the  new  results  arc 
used  to  find  bounds  on  RHC  parameters  such  as  prediction  horizon  and  execution 
horizon.  This  provides  a  more  systematic  way  for  choosing  decentralized  RHC 
parameters  so  that  stability  is  guaranteed.  Also,  since  the  communication  induces  a  delay 
to  the  system,  a  new  formulation  of  decentralized  RHC  is  proposed  which  accounts  for 
large  communication  delays.  Two  different  case  studies  on  the  formation  of  mobile 
robots  and  formation  flight  of  UAVs  have  been  investigated  to  clarify  the  problem 
statement. 
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Nomenclature 


A,  B  :  Linear  system  matrix 
E  :  Set  of  edges  (neighbour  vehicles) 

G  :  Graph  topology 

//  ;  Hamiltonian  equation 

;  Yawing  Moment  of  inertia 
J  :  Cost  function 

Lx,  Lu  :  Lipchitz  constant 
l,r  :  Tail  rotor  hub  location  behind  c.g 
Nv  :  Number  of  vehicles 

P,  Q,  and  R  :  Weight  matrices  of  quadratic  cost  function 

q  :  Costate  Vector 

Rc  :  Communication  radius 

Rs  :  Safety  radius 

r.  .•  Yawing  Rate 

5/“* ;  Frontal  fuselage  drag  area 

T  :  Prediction  horizon  (or  finite  horizon  time) 

u  :  vector  of  control  inputs 

V  :  Set  of  vehicles 

X  :  Vector  of  states 

P  :  Model  uncertainty 

if/  :  Euler  angle  (Yaw  angle) 

6  ;  Execution  horizon  (sampling  time) 

:  Uncertainty  coefficient 
rj  :  Communication  delay 


Superscript 

*  =  Optimal  value  of  parameter 


Subscript 

actual  =  Actual  system 

predicted  =  Predicted  value  of  parameter 


6-3 


Chapter  6:  A  V'ariable  Communication  Approach  for  Decentralized  Receding  Horizon  Control  of  Multi-Vehicle  Systems 


6.1  Introduction 

This  report  addresses  the  following  task  related  to  the  project  “Synthesis  and 
Implementation  of  Single  and  Multi-vehicle  Systems  Guidance  Based  on  Nonlinear 
Control  and  Optimization  Techniques”  and  the  following  task; 

Task  10:  To  validate  theoretically  the  feasibility  and  existence  of  solutions,  the 
stability,  and  the  robustness  of  the  RHC  despite  an  adversarial  and  partially  unknown 
environment. 

Over  the  past  few  decades  there  has  been  significant  interest  in  decentralized  control  of 
cooperative  problems''^.  The  decentralized  structure  potentially  results  in  increased 
autonomy  of  agents  while  providing  some  sort  of  cooperation  among  the  agents  to 
perform  a  common  mission.  In  fact,  the  main  idea  with  decentralized  control  is  to  break 
the  centralized  controller  into  the  local  control  problems  of  smaller  sizes  in  order  to 
reduce  the  computation  effort  and  communication  requirements. 

However,  decentralization  must  provide  and  respect  the  primary  objectives  of 
cooperative  control  problem,  these  primary  objectives  are:  1-Thc  total  cost  added  due  to 
cooperation  must  provide  a  greater  increase  in  expected  system  effectiveness  than  the 
case  of  the  non-cooperating  vehicles.  2-  Performance  lower  bound  of  a  cooperative 
system  as  communication  degrades  should  never  be  worse  than  the  performance  of  the 
same  vehicles  without  cooperation.  Since  these  primary  goals  of  the  cooperative 
architectures  arc  not  achieved  yet  through  decentralization,  the  researchers  arc  motivated 
to  investigate  new  potential  ways  for  further  improvements  of  the  decentralized  control 
architectures.  For  example,  most  recently  with  recent  advances  in  distributed 
computation,  there  have  been  numerous  attempts  to  use  computational  expensive 
optimization  based  control  methods  such  as  receding  horizon  control  (RHC)  '  tor 
decentralizing  the  cooperative  problems.  Although  RHC  is  computationally  expensive,  it 
has  some  prominent  capabilities  which  motivate  the  researchers  to  develop  the  RHC 
based  decentralized  control  architectures!'  For  example,  RHC  readily  handles 
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systems  constraints  and  saturations  while  providing  optimal  control  action.  Also,  since  in 
RHC  the  control  action  is  generated  through  minimization  of  a  cost  function  it  is  easy  to 
provide  a  coupling  between  neighbour  vehicles  in  the  cost  function  or  constraints. 

Another  potential  way,  which  is  also  one  step  ahead,  is  to  use  the  communication 
based  approaches  for  stability  enhancement  of  the  decentralized  RHC  architectures;  this 
approach  is  the  main  focus  of  this  report.  Using  the  variable  communication,  it  is  desired 
to  use  the  maximum  available  communication  capacity  to  enhance  the  stability  and 
performance  of  the  decentralized  RHC  architecture.  Although  most  of  the  research  works 
in  the  field  of  decentralized  RHC  have  focused  on  modifying  the  cost  function  and 
constraints,  they  lack  utilizing  communication  methods  efficiently  for  further 
enhancement  of  stability.  It  is  also  noticeable  that  since  most  of  these  research  works 
persist  on  reducing  the  communication  requirement  they  sacrifice  the  benefits  of 
communication  capabilities  and  don’t  allow  using  advanced  communication 
technologies.  Hence,  new  decentralized  RHC  algorithms  arc  required  to  utilize  the 
maximum  available  communication  capacity.  Since  in  the  literatures  this  mechanism  is 
not  thoroughly  addressed,  in  this  report,  the  role  of  the  communication  for  improving  the 
stability  of  the  decentralized  RHC  algorithm  is  investigated.  Also,  an  algorithm  is 
proposed  which  enables  vehicles  to  find  the  best  combination  of  communication  topology 
and  RHC  parameters  while  the  stability  is  ensured. 

In  the  decentralized  RHC,  a  centralized  RHC  controller  is  broken  into  the  local 
control  problems  of  smaller  sizes  To  capture  the  nature  of  cooperation  among  vehicles,  a 
coupling  between  neighbour  vehicles  is  provided  in  the  formulation  either  in  the  cost 
functionl'  ^  or  in  the  constraints*’.  However,  control  actions  computed  locally  are  not 
guaranteed  to  be  globally  feasible  in  general  1  and  to  stabilize  the  team.  In  general, 
stability  and  feasibility  of  the  decentralized  RHC  methods  arc  difficult  to  prove  and  may 
yield  poor  performance"^''^. 

As  in  classical  RHC,  most  of  the  researchers  have  addressed  the  stability  and 
feasibility  of  the  decentralized  RHC  by  modifying  the  cost  function  and  constraints.  For 
example,  a  sophisticated  work  is  conducted  by  Keviezky  et  al.l,  where  a  decentralized 
RHC  is  proposed  for  decoupled  discrete  time  systems  by  breaking  down  a  centralized 
RHC  architecture  into  the  distinct  RHC  controllers  of  smaller  sizes.  Each  RHC  controller 
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is  associated  to  a  different  vehicle  and  computes  the  local  control  inputs  based  only  on 
the  states  of  itself  and  its  neighbours.  In  this  approach,  the  vehicles  are  coupled  through  a 
cost  function  and  the  necessary  information  from  the  neighbour  vehicles  is  provided 
through  non-delay  eommunieation  or  measurement.  Each  vehicle  predicts  its  neighbours’ 
behaviour  from  their  dynamieal  model  and  based  on  this  prediction  plans  the  trajectory  of 
itself  and  its  neighbours,  but  executes  its  own  trajectory  and  discards  the  trajectories  of 
neighbours.  It  is  proven  that  if  the  mismatch  between  the  predicted  and  actual  trajectories 
of  all  neighbours  is  smaller  than  some  cost  function  related  to  the  initial  conditions  then 
the  global  group  stability  is  achieved.  In  another  work,  Dunbar  et  al.  proposed  a 
distributed  RHC  for  multi  vehicles  with  continuous  time  dynamically  decoupled 
subsystems  whose  state  vectors  are  coupled  through  the  cost  function  of  a  RHC  control 
problem*.  Each  vehicle  solves  an  optimization  problem  and  generates  its  control  action 
using  an  assumed  control  action  of  neighbour  vehicles.  The  key  requirement  for  stability 
is  that  each  distributed  optimal  control  not  deviate  too  far  from  the  previous  one;  this 
constraint  restricts  the  optimization  problem  and  doesn’t  allow  very  fast  maneuvers. 
Also,  the  communication  delay  is  not  addressed. 

As  an  example  of  constraint  coupled  decentralized  RHC  architecture  one  can  refer  to 
[6]  and  [10]  where  authors  decentralized  a  previously  developed  robust  safe  but 
knowledgeable  (RSBK)  model  predictive  control  algorithm  [11]  that  uses  the  constraint 
tightening  technique  to  achieve  robustness  and  lower  online  computation  burden.  Using 
local  knowledge  of  the  group,  it  is  ensured  that  each  vehicle  always  has  a  solution 
guaranteeing  robust  feasibility  of  the  entire  fleet  under  the  action  of  disturbances.  The 
algorithm  is  extended  in  [12],  to  explicitly  account  for  the  communication  and 
computation  delays  in  the  system  in  the  presence  of  bounded  disturbances. 
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Fig.  1 ;  Communication  radius  and  safety  radius 

Our  analysis  shows  that  combining  the  stability  results  with  variable  communication 
can  lead  to  stability  enhancement  of  the  decentralized  RHC  approach  for  more  realistic 
cases.  To  discuss  the  variable  communication  methods,  some  concepts  regarding  the 
eommunication  such  as  communication  radius  and  multi-hopping  communication  arc 
introduced  and  investigated  to  exploit  this  mechanism.  The  communication  radius  (figure 
1)  defines  a  region  around  each  vehicle  where  communication  with  vehicles  which  arc 
inside  this  region  is  possible.  Multi-hopping  communication  will  be  introduced  later  in 
section  5.1.  In  this  report,  it  is  studied  how  the  following  three  techniques  can  enhance 
the  stability  of  the  decentralized  RHC  method:  1-  faster  communication  rates  2-  enlarging 
the  communication  radius,  3-  multi-hopping  communication.  Based  on  these 
investigations  a  communication  based  algorithm  is  proposed  for  further  stability 
enhancement  of  decentralized  RHC.  The  main  idea  with  the  proposed  method  is  to 
improve  the  communication  topology  and  select  the  decentralized  RHC  parameters 
through  using  the  maximum  available  communication  capacity  so  that  the  overall 
stability  and  performance  is  guaranteed. 

The  main  contributions  of  this  report  arc  presented  as  follows:  first  a  new  formulation 
of  decentralized  RHC  is  established  for  continuous-time  dynamically  decoupled  systems. 
This  new  formulation  accounts  for  communication  delays  larger  than  sampling  time. 
Also,  this  formulation  allows  using  a  time  varying  graph  topology  along  with  a  time 
varying  cost  function  for  representing  a  time  varying  coupling,  cooperation  and 
communication  topology  among  vehicles.  Secondly,  a  stability  condition  is  derived  for 
the  new  time  varying  decentralized  RHC  formulation;  through  driving  the  stability 
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condition  a  bound  is  found  on  the  differentiation  of  the  cost  function  which  is  treated  as 
the  Lyapanov  function  candidate.  This  bound  allows  defining  a  stability  margin  to  have  a 
measure  of  stability  and  performance.  Also,  this  stability  eondition  allows  studying  the 
variable  eommunication.  Then,  based  on  the  derived  stability  eondition,  the  effeet  of 
faster  eommunieation  rate,  larger  communieation  radius,  and  multi-hopping 
eommunication  on  the  stability  is  investigated.  Using  the  extension  of  the  Bellman- 
Grownwall  lemma  along  with  the  advantages  of  Hamiltonian  equation,  the  acceptable 
region  for  the  prediction  horizon  and  execution  horizon  (eommunication  rate)  arc 
determined  that  allows  tuning  RHC  in  a  systematic  way  so  that  the  stability  is  guaranteed. 
The  results  of  these  analyses  finally  lead  to  proposing  a  new  decentralized  RHC 
algorithm  which  allows  choosing  the  optimum  communication  topology  along  with  the 
sampling  time  and  prediction  horizon.  This  algorithm  enables  each  vehicle  to  use  the 
maximum  available  communication  capacity  for  stability  enhancement  of  decentralized 
RHC  architecture.  The  last  part  involves  two  examples  which  clarify  the  connection 
between  the  proposed  approach  and  realistic  scenarios.  The  first  example  is  the 
simulation  of  a  group  of  5  robots  that  verifies  the  effectiveness  of  proposed  method  for 
enhancement  of  the  stability  and  performance  by  modifying  the  communication  topology. 
And,  the  second  example  involves  the  simulation  of  a  feet  of  three  rotorcraft  UAVs;  the 
selected  scenario  shows  how  in  the  real  cases  we  need  the  variable  communication  to 
perform  the  missions.  More  precisely,  in  the  selected  scenarios  the  vehicles  needs  to 
change  the  communieation  topology  in  order  to  avoid  obstacles  or  visiting  the  far  targets. 
The  contribution  of  this  research  work  has  been  four  conference  papers  so  far 

6.2  Problem  Formulation 

6.2.1  Problem  Statement 

As  it  is  mentioned  previously,  the  main  objective  of  this  report  is  to  develop  a  new 
algorithm  so  that  the  maximum  available  communication  capacity  can  be  used  for 
stability  enhancement  of  decentralized  RHC  architecture.  More  specifically,  an  algorithm 
is  developed  which  optimizes  the  communication  topology,  communication  rate  and 
prediction  horizon  at  each  time  step  using  variable  communication.  The  reason  for  using 
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the  variable  communieation  is  to  cover  a  more  genera!  set  of  real  scenarios;  because  in 
the  real  scenarios  a  fixed  communication  topology  can  not  be  used  most  of  the  time.  For 
example,  figure  2  shows  a  formation  problem  in  which  the  UAVs  leave  the  formation  due 
to  visiting  the  far  targets.  When  the  vehicles  flight  in  the  formation  they  can 
communicate  with  each  other  but  when  they  leave  the  formation  they  may  get  beyond  the 
communication  radius  and  won’t  be  able  to  communicate  with  each  other.  Hence,  a 
variable  communication  topology  is  needed  in  this  case  for  mathematical  representation 
and  studying  of  scenario. 


®  Target  position 


I 

V 


UAVs  leave  the  formation  in 
order  to  visit  the  targets;  they 
make  smaller  subgroups 


Fig.  2:  Formation  of  a  fleet  of  UAVs:  a  time  varying  communication  topology  due  to 

far  targets 

The  same  scenario  may  happen  where  the  obstacles  and  non-fly  zones  in  the  battle  field 
drive  the  vehicles  to  go  far  away  of  each  other  and  change  the  graph  topology.  In  this 
report  one  example  of  such  problems  is  discussed. 

The  following  diagram  shows  how  this  report  is  organized  to  develop  the  new 
algorithm  and  how  the  result  of  each  section  is  related  to  other  sections: 
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Variable  Decentralized  RIIC 
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Modified  Algorithm 
for  Decentralized 


Fig.  3:  Schematic  representation  of  the  proposed  approach  for  stability 

enhancement 

This  diagram  says  that  to  enhance  the  stability  a  variable  communication  method  is 
used.  A  variable  communication  requires  that  the  communication  topology, 
communication  rate  and  decentralized  RMC  be  time  varying  as  well.  The  following 
explains  how  these  variations  interact  and  affect  each  other: 

1-  V'ariable  Communication  Topology:  Variable  communication  topology  is  used 
whenever  the  stability  and  performance  of  the  decentralized  RHC  arc  not  satisfactory. 
Although  the  measurement  of  stability  and  performance  is  not  the  subject  of  this  report,  a 
stability  margin  is  defined  in  this  report  to  have  a  measure  of  stability.  The  measure  of 
performance  depends  on  the  specification  of  each  problem,  requested  mission  and  desired 
performance  index.  For  example,  in  a  path  planning  problem  it  is  desired  to  visit  some 
targets  without  colliding  the  obstacles;  hence,  the  measure  of  performance  comes  from 
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the  fact  that  how  close  the  vehicle  get  to  the  target  and  how  better  they  can  respect  the 
limitations. 

Once  the  stability  and  performance  of  the  decentralized  RHC  are  not  satisfactory  a 
modification  in  the  communication  topology  is  sought  through:  1-  Changing  the 
communication  radius  and  2-  Using  the  multi-hopping  communication.  This  variation  in 
the  communication  topology  imposes  that  the  decentralized  RHC  architecture  be  time 
varying  because  the  decentralized  RHC  is  formed  at  each  update  based  on  the  current 
communication  topology. 

2-  Variable  Decentralized  RHC:  A  time  varying  llexible  formulation  of 
decentralized  RHC  is  required  for  representation  of  time-varying  parameters.  This  time 
varying  formulation  is  discussed  in  section  3.2  and  includes  the  time  varying:  1-  Cost 
function,  2-  Prediction  horizon  and  3-  Execution  horizon.  Since  the  execution  horizon  is 
equal  to  the  communication  sampling  time,  the  time  variation  of  execution  time  implies 
the  time  variation  of  the  communication  sampling  time;  hence,  the  communication  rate 
must  be  also  taken  into  account.  The  related  algorithm  for  online  implementation  of  this 
time-varying  formulation  is  provided  in  section  3.3  and  is  extended  in  3.4  to  account  for 
communication  delay. 

3-  Parameter  Selection:  Once  a  topology  is  selected  it  is  required  to  answer  two 
major  questions:  1-  What  is  the  best  communication  rate?  2-  What  is  the  best  prediction 
horizon  for  decentralized  RHC?  These  two  questions  are  answered  in  section  5.2  and  5.3 
for  the  case  of  model  uncertainty  and  no  model  uncertainty.  For  each  communication 
topology  there  exist  an  optimum  value  of  communication  rate  and  prediction  horizon, 
these  values  must  respect  the  stability  condition  of  decentralized  RHC  and 
communication  limitations.  Hence  a  stability  analysis  is  required  for  the  variable 
decentralized  RHC  architecture  so  that  it  yields  a  suitable  stability  condition.  After 
modification  of  the  communication  topology  the  results  of  this  section  will  be  used  to 
select  the  corresponding  optimum  value  of  the  sampling  time  and  prediction  horizon. 
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4-  Modified  Algorithm  for  Decentralized  RHC:  Based  on  the  results  from  the 
variable  eommunieation  and  stability  analysis  of  the  decentralized  RHC  a  new  algorithm 
is  proposed  for  the  online  optimization  of  the  communication  topology  so  that  the 
stability  enhancement  is  achieved  through  using  the  maximum  available  communication 
capacity. 

To  address  the  mentioned  issues  we  follow  the  steps  below: 

1-  Formulation  of  time  varying  communication  topology  and  cost  function. 

2-  Formulation  of  time  varying  decentralized  RHC  considering  the  case  of 

communication  delay. 

3-  Stability  analysis  of  time  varying  decentralized  RHC  and  providing  a  stability 

condition  and  stability  margin  to  have  a  measure  of  stability. 

4-  Investigating  the  effect  of  expanding  the  communication  radius  on  the  stability 

margin. 

5-  Investigating  the  effect  of  multi-hopping  communication  on  the  stability  margin. 

6-  Investigating  the  effect  oi prediction  horizon  on  the  stability  margin. 

7-  Investigating  the  effect  of  faster  communication  rate  (execution  horizon)  on  the 

stability  margin. 

8-  Proposing  an  algorithm  for  optimum  selection  of  communication  topology, 

communication  rate  and  prediction  horizon. 

9-  Proposing  a  modified  decentralized  RHC  algorithm. 

These  steps  arc  shown  in  the  following  diagram  and  the  results  of  each  step  are 
mentioned  in  the  right  side  of  each  box,  the  dotted  arrows  shows  the  relations  between 
the  results  of  each  step  and  how  the  results  drive  each  other: 
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Fig.  4:  Schematic  representation  of  the  proposed  approach  for  stability 

enhancement 

6.2,2  Decentralized  RHC  Formulation 

6.2.2. 1  Time  varying  communication  topology: 

A  time  varying  graph  topology  is  introduced  to  present  the  coupling  and  cooperation 
among  the  vehicles.  Consider  a  set  on  vehicle  cooperating  to  perform  a  common 
mission,  the  i-th  vehicle  is  associated  to  the  i-th  node  of  the  graph  and  if  an  edge  (i,  j) 
connecting  the  i-th  and  j-th  node  is  present,  it  means  the  i-th  and  j-th  vehicle  are 
neighbour.  This  leads  to  an  interconnection  graph  as  follows: 

G(t)^  {V  (1) 
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Where,  V  is  the  set  of  nodes  (vehicles)  and  £(/)c  the  set  of  time  varying  edges  (i,  j), 
with/.yeK.  The  interconnection  graph  is  undirected  i.c.  (/,y)  e  £’(/)  implies 
(y,/)  e  £(f)  even  though  it  is  not  appear  in£’(f).  Also,  the  interconnection  constraint 
between  neighbour  vehicles  in  the  group  (the  i-th  and  the  j-th  vehicles)  is  presented  as 
follows: 

g,(^,.v,)<0  (2) 

Different  ways  for  defining  the  neighbour  set  and  connectedness  of  graph  topologies 
have  been  investigated  in  [’’].  Here  we  assume  that  the  time  dependence  of  the  set  of 
edges  is  a  function  of  the  relative  distance  of  the  vehicles;  hence,  the  set  E(0  is  defined  as 
follows: 

E{t)  =  {{iJ)eV  xv  \  <  /?,. }  (3) 

where,  denotes  the  distance  between  i-th  and  j-th  vehicles,  and/?,^  is  the 

communication  radius  of  vehicles  (figure  1).  Furthermore,  to  ensure  the  collision 
avoidance  it  is  required  to: 

d.,  ^  (4) 

where,  is  the  safety  radius  around  each  vehicle  and  it  is  shown  in  figure  1.  The 

safety  radius  defines  a  region  around  each  vehicle  that  neighbours  aren’t  allowed  to  get 
inside  this  region  otherwise  the  collision  may  occur.  This  constraint  is  added  to  the 
formulation  of  the  decentralized  RHC  problem  to  prevent  the  vehicles  from  colliding 
each  other.  Defining  the  following  set  for  each  vehicle  helps  representing  the  coupling  in 
the  next  section: 

£'(0  =  {(/,y)eFxF|  ^,<4(0}  (5) 

Where  R‘^(t)  is  the  communication  radius  of  i-th  vehicle  at  time  t.  This  time  varying 
graph  topology  enables  us  to  represent  all  time  varying  configurations  of  the  subgroups. 
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6.2. 2. 2  Time  varying  cost  function: 

The  vehicles  may  also  be  coupled  in  the  cost  function;  then  to  cover  a  generic  set  of 
cooperative  control  problems,  the  following  time  varying  cost  function  is  proposed  for 
the  i-th  vehicle; 


X  4,, (.v^(/), »'(•)) 


(6) 


Where,  T(t)  is  the  prediction  horizon  of  i-th  vehicle  at  time  t  and: 

x‘{t)  =  {.r'(0}  e  ^(0} 

ir(t)  =  {n'il)}yj{ii'it)\(iJ)e  E{l)} 

The  first  term  in  (6)  is  associated  to  the  individual  vehicle  and  the  second  term  is 
associated  to  the  neighbour  vehicles  and  represents  the  coupling  between  the  i-th  vehicle 
and  its  neighbours  i.c.  it  is  defined  on  the  set  of  neighbours  of  i-th  vehicle.  Since  Eft)  is 
time  varying  then  the  cost  function  becomes  also  time  varying..  The  cost  function  is 

defined  over  the  prediction  horizon  time  T ft)  for  each  vehicle. 


Remark:  In  a  centralized  RHC  problem  the  cost  function  is  formed  as  the  summation 
of  all  individual  cost  functions  ( )  of  all  vehicles.  In  fact,  the  optimization  problem  is 

solved  for  all  vehicles  by  a  central  agent  and  the  planned  trajectory  for  each  vehicle  is 
transmitted  to  it. 


In  our  case  the  cost  function  for  each  vehicle  is  defined  as  following  for  the 
dynamically  decoupled  continuous  time  systems: 


t  tT'  [I) 


L‘ 


T'H) 


(8) 


Where,  .vj(r)  and  //'(r)  are  the  state  vector  and  control  vector  of  i-th  vehicle  respectively 
at  time  r ,  calculated  from  the  optimization  problem  started  at  time  t.  Also,  P.  Q  and  R 
are  positive  definite  and  symmetric  matrix  penalties.  This  kind  of  cost  function  allows 
formulation  of  more  generic  set  of  problems  and  is  used  widely  in  the  literatures  1' 
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Moreover,  this  it  is  possible  to  benefit  from  the  unique  properties  of  quadratie  cost 

1 8 

functions  in  the  optimization  problems  . 


6.3  Variable  Decentralized  RHC: 

In  the  RHC  method  a  cost  function  is  optimized  over  a  finite  time  called  prediction 
horizon,  r ,  and  the  first  portion  of  the  computed  optimal  input  is  applied  to  the  plant 
during  a  period  of  time  called  the  execution  horizon,  s ;  repeating  this  procedure  yields  a 
closed  loop  solution,  the  reader  is  referred  to  [**]  for  a  comprehensive  review  of  RHC 
literatures.  Suppose  that  the  following  represents  the  nonlinear  decoupled  dynamics  of 
the  i-th  vehicles: 

x'(t)  =  f{x‘{t),u‘(t))  (9) 

We  will  define  optimization  problem  P'(t)  for  the  i-th  vehicle  as  follows: 


Variable  Decentralized  RHC  Problem  P'(/) : 

min  (10) 

Subject  to: 

x‘{t)  =  fix'{t),u'{t))  -x‘  eX\u'  eU* 

x'0)  =  ;x'  eX',iv' &U‘,  (i,j)&E{l) 

g‘-^  (,v'  (f ), //'  (f ), ^ '  (f ),  /y '  (f )) <  0, (/,  j)  e  £(f ) 

.v'(0)eA'o,»'(0)eC; 

jr'(0)eA'',(£(0)eC„',(;,y)e£(f) 

x‘{T)eX 

x'(T)sX  ;,iE{T)sU',Ai,j)sE(l) 


Where  (y)is  defined  in  (6),  A"  and  (/'denotes  the  set  of  acceptable  states  and  inputs 
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respectively  for  i-th  vehicle,  X'^and  X'^  are  the  set  of  acceptable  initial  and  final  states 
of  i-th  vehicle  respectively. 


Assumption:  Without  losing  the  generality,  it  is  assumed  that  the  equilibrium  point  of  the 
system  (vehicle’s  dynamics)  is  origin.  This  assumption  is  not  restrictive  because  in  the 
cases  where  the  equilibrium  point  is  not  origin  a  change  in  the  variables  can  be  used  to 
transfer  the  equilibrium  point  to  the  origin. 


Assumption:  It  is  assumed  that  is  selected  as  small  enough  so  that  there  exist  a 
feedback  control  that  drives  x' (/ +  7’'(/))  to  the  origin.  If  such  a  feedback  control  for 
/’'(/)  exist  the  optimization  problem  P'(/)  is  feasible. 


The  control  action  obtained  from  this  optimization  problem  is  implemented  during  the 
execution  time  S' (t)  e[t,t  +  T' (t)]  till  the  next  update.  Repeating  this  procedure  online 
yields  a  close  loop  solution. 
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6.3.1  Variable  Decentralized  RHC  Algorithm 

In  order  to  solve  the  above  optimization  problem,  each  vehicle  needs  to  know  current 
states,  model,  constraints  and  tenninal  region  of  itself  and  of  its  neighbours.  Based  on 
such  information  each  vehicle  computes  the  optimal  inputs  of  itself  and  of  its  neighbours. 
The  input  to  the  neighbours  will  only  be  used  to  predict  their  trajectories  and  then 
discarded,  while  the  first  portion  of  the  i-th  optimal  input  of  problem  P'{t^)w\\\  be 
implemented  to  the  i-th  vehicle  during  where  )  ,A- gC  and 

c>'(/4 ) denotes  the  RHC  sampling  period  and  also  called  execution  horizon.  The  following 
algorithm  is  proposed  for  the  on-line  implementation  of  the  above  optimization  problem. 
The  algorithm  is  formulated  for  the  i-th  vehicle;  in  fact,  all  vehicles  run  this  algorithm 
during  performing  the  mission  simultaneously: 

V'ariable  Decentralized  RHC  Algorithm  1: 

1)  Let  A=0 

2)  Compute  graph  connection  £"(f^ )  according  to  (5). 

3)  Sample 

4)  Communicate  -C»,u/(C  )to  neighbour  vehicles  and  receive.vj^,,„^,(Z^ ) 
for(z,y)e 

5)  Solve  the  decentralized  optimization  problem  and  generate  the  control 

action  for  time  interval  [Z^ ,  Z^  +T'  {t^ )] . 

6)  Execute  the  control  action  for  individual  vehicle  over  the  time 
interval  [z*,z*,|]. 

«/'(0  =  t  e  02) 

7)  A-=A  +  /.  Goto  step  2. 

This  algorithm  is  repeated  till x' (Z)  =  0  (assuming  the  origin  is  the  equilibrium  of  the 


6-18 


Chapter  6:  A  Variable  Communication  Approach  for  Decentralized  Receding  Horizon  Control  of  Multi-Vehicle  Systems 


system).  The  6'*’  step  of  the  algorithm  allows  the  vehicles  to  share  and  update  their 
information  about  the  targets. 


6.3.2  Variable  Decentralized  RHC  Algorithm  with  Communication  delay 

Since  in  the  decentralized  RJIC  the  neighbour  vehicles  arc  coupled  through  the  cost 
function  and  constraints,  each  vehicle  needs  the  updated  information  of  its  neighbours 
frequently  at  each  sampling  time  )  (execution  time)  to  solve  the  optimization  problem 

P'  (fj )  and  to  generate  its  trajectory. 

On  the  other  hand,  in  reality  the  communicated  information  is  subject  to  delay; 
therefore,  even  in  the  case  of  availability  of  communication,  the  current  infomiation  ot 
neighbour  vehicles  is  not  available  for  all  vehicles,  c.g.  the  current  states  of  the 
neighbours  of  i-th  vehicle  is  not  available  for  i-th  vehicle.  To  account  lor  the 
communication  delay,  prediction  must  be  done  based  on  the  most  recent  received 
information  from  neighbours.  In  cases  where  the  communication  delay  is  smaller  than 
execution  horizon  the  prediction  problem  is  straightforward  by  communicating  the 
control  action  and  predicting  the  state  of  neighbours  from  their  mathematical  model. 

However,  in  some  applications  in  the  real  world  the  communication  delay  is  bigger 
than  execution  horizon  (or  even  prediction  horizon).  The  communication  delay  comes 
from  different  sources;  for  instance,  the  limited  band  width  of  the  communication 
facilities  can  induce  a  delay  in  the  communicated  data.  Also,  the  slow  sensors  and/or  the 
process  of  send  and  receive  can  cause  a  bigger  communication  delay.  For  another 
example  one  can  point  to  the  case  of  submarines;  in  the  water  the  communicated  signals 
can  not  travel  as  fast  as  the  communicated  signals  in  the  air;  hence,  the  communication  in 
the  water  involves  a  bigger  delay  than  the  air.  In  these  cases  a  new  algorithm  ol 
decentralized  RHC  is  required  to  account  for  communication  delay.  Suppose  that  the 
communication  delay  between  i-th  and  j-th  vehicle  is  denoted  by 
r''and?j_j  -r''  c/eD  ;  also,r'^  <m'm{r(t^),T'{i^_j))  ■ 
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The  following  algorithm  is  proposed  for  the  decentralized  RHC  algorithm.  The 
algorithm  is  developed  for  the  vehicle;  in  fact,  all  vehicles  run  this  algorithm  during 
performing  the  mission  simultaneously: 

Variable  Decentralized  RHC  Algorithm2: 

1)  Lc\  k=0. 

2)  Compute  graph  connection  ^'(/^ )  according  to  (5). 

3)  Sample  „,,(/*). 

4)  Communicate  (/< )  to  neighbour  vehicles  and  receive  where 

(/,  j)  G  E'  (fj )  and  goto  step  7 

5)  Solve  the  decentralized  optimization  problem  and  generate  the  control 

action  for  time  interval  [/^ ,  f*  +  T'  )] . 

6)  Execute  the  control  action  for  individual  vehicle  over  the  time 
interval 

iE{t)  =  »;;(/);  /  G  (13) 

7)  A'= A +  7.  Goto  step  2. 


In  step  5  of  the  above  algorithm  the  state  of  neighbour  vehicles  at  4  is  needed  to  solve  the 
optimization  problem  p,  while  this  information  is  not  available  due  to  communication 
delay.  The  following  procedure  is  proposed  to  predict  .r '  ' (f^, )  where  .v,'''(/)  denotes  the 
state  of  j-th  vehicle  at  time  step  t,  computed  by  i-th  vehicle  through  solving  the 
optimization  problem  P‘{t^ ) : 

At  /  =  fj  the  following  information  from  the  neighbours  arc  available: 

Hence,  solving  the  following  ordinary  differential  equation  yields  the  solution: 
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X 


J 


f{x\u'); 


d)  ~  d  ) 

\ii^'‘{s)  =ui-!,,,{s) -,3  e 


(14 


Remark:  If  r''  <5\t^_j)<5^  {s)^u'’  {s)  \  therefore,  if  there  is  no  model 

uncertainty  =  Generally,  if  then 

if '■'(.s');tw''(-v)  because  i-th  vehicle  uses  some  portions  of 
+  from  the  previous  optimization  problem  while  j-th 

vehicle  uses  the  updated  control  action.  Therefore,  even  in  the  case  ot  no  model 
uncertainty  it  may  lead  to  a  mismatch  between  and  x"'"' (/^ )  which  induces  a 

mismatch  between  the  trajectories  that  P'(fjand  generate  for  the  j-th  vehicle 

during[f^,/^ -i-r'(f*)],  this  may  lead  to  infeasibility  and  instability  of  the 

following  sections  will  address  this  issue  and  finds  a  bounds  on  the  control  action  and 
mismatch  to  ensure  the  feasibility  and  stability  of  the  decentralized  RHC  problem. 


6.4  Stability  condition  of  Decentralized  RHC  for  continuous  time 
systems 

In  the  decentralized  RHC  formulated  here,  the  vehicles  use  a  model  of  the  neighbour 
vehicles  to  predict  their  trajectory.  The  predicted  neighbours'  trajectories  will  be  used  to 
meet  the  sufficient  condition  for  stability  of  the  whole  group.  In  addition,  the  information 
communicated  between  the  vehicles  is  used  after  each  sampling  time  to  make  the 
predicted  solutions  feasible.  The  following  proposition  gives  the  sufficient  condition  lor 
the  stability  of  the  decentralized  RHC  problem . 

Proposition  1:  Suppose  that  matrix  penalties  Q  and  R  arc  symmetric,  positive  and  full 
rank.  Also,  assume  the  sets  of  states  and  inputs  contain  the  origin.  Then,  the  following 
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inequality  is  the  sufficient  condition  for  the  asymptotic  stability  of  the  decentralized  RllC 
problem  P'(/^ )  at  the  origin: 


where  e‘  is  the  summation  of  the  prediction  mismatches  over  the  prediction  horizon: 


•) 

'^dt+  j  (/)-»/;(/) 

'  dt 

Q 

7](/,y  )££■((,) 

^+1 

^+1 

(15) 


(16) 


Also: 


iP 

1  <1 

)dt 

R 

h 

(17) 


Proof:  Without  losing  the  generality  consider  the  case  of  two  vehicles:  (/=/,y-2);  then: 

<1 .1  ♦  r'  ('• ) 


(18) 


Also: 


t,*T 


J  (||'V,';'(')||  +  +  ||.r;;'(f,  +t 


(19 


*r 


+  T 


Also  consider  the  optimization  problem  P'(fjwith  the  following  optimal  solutions: 

w,';'*  (.O  ;  ^  e  [f*  ,  f*  +  r  '  (f*  )] 
u;-'’  {s)\  J  6  [f*  ,  f*  +  r  '  (^  )] 


(20) 


^rid  M,^‘(/^^,)are  calculated  from  two  different  optimization  problems  /’’(tj 
and  respectively;  hence,  they  are  not  necessarily  the  same  and  it  means  the 
following  shifted  optimization  solutions  may  not  be  feasible: 


6-22 


Chapter  6:  A  Variable  Communication  Approach  for  Decentralized  Receding  Horizon  Control  of  MultiA'ehicle  Systems 


[O  ;.v6  [f*.,  +  T  -  +  7’'(/,.,)] 

I  (5)  ;  5  6  [f*,,  ,  +  r  '  (/j,|  )  -  r>'  '  (fj,,  )] 


(21) 


Hence,  we  construct  the  following  feasible  control  trajectory: 


jo  ;.ve  [/,,,  +  7-'(M  -  +  7"(/*^,)] 

e  [f*,,,  f*,,  +  7- '(/*)-  SU',)] 

-  0  6  [/*,,  + 

0  6  [f,,,  +  r'(fj-  +  T'if,,,)] 


(22) 


Since  the  dynamics  of  the  vehicles  arc  not  coupled  it  is  possible  to  construct  this  feasible 
solution.  This  feasible  control  trajectory  will  be  used  to  calculate  the  cost  function  (23) 
which  is  not  optimal  necessarily: 

2 
P 

(23) 

'i.i 

Also,  since  we  assumed  the  dynamics  are  decoupled,  and  arc  feasible  at  time 
t  =  /^^,  for  problem  P' ) .  The  figure  below  shows  schematically  the  three  cost  function 
which  will  be  used  to  prove  the  stability: 


.  I  +  .  I  ) 


).»'(•))=  J  (|K''(o||^  +  +  +  7’U^ 


I 


.2.1 
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Fig.  5;  Schematic  representation  of  cost  functions  used  to  prove  the  stability 


Hence,  from  the  optimality  property  we  have: 


-  •  1 

,  (  r  \  I .  .  ) .  * 

t/* 


r  I *1  )  >  "  +1  ~  r ^  1 1  (/*  ♦  I  ) ) 


■  +  «;;'(0  \  )dt  -  x]\i,  +  T'{, 


'l-l 


*  J 


xl^it 


||.v,V'(0|hc//+  j  (lKV^(0|r 


!//;'{?)  ^  )di 


+  J  [\\x 
/.  -r'  ('.  ) 

.1.1/,.  .  TT  1 


(24) 


If  matrix  penalty  P  is  calculated  from  the  control  Lyapanov  function  (CLF)  of  the 
linearized  model  of  (9),  then  the  final  cost  ||4/*  +7’)||pis  a  bound  on  the  finite  optimal 
cost  function'^;  in  fact: 


(25) 


1  (|K'(')|ly  + 


Hence: 


IC,  (9.,  +  7"(^.,))C  +  -  |K'(^  +  ^'(9  ))t  -|K  '(9  +  + 


J 


I,  ) 


(26) 


Using  (26)  in  (24)  and  using  triangle  inequality  for  the  weighted  norms  yields: 
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It  {It  ) 


U  ^T'Ht  ) 


+  J  +  I  |K'/(o  -  «,V'(o||p 


(27) 


According  to  Lyapanov  stability  analysis  lor  the  stability  of  the  decentralized  RIIC,  the 
right  hand  side  of  inequality  (27)  must  be  negative  which  yields  the  following  stability 
condition  for  the  case  of  two  vehicles: 


I,  I 


'i.i 


(28) 


A  same  analysis  for  the  general  case  with  Nl  number  ol  neighbours  for  i-th  vehicle 

yields  the  general  stability  condition  (15).  Stability  condition  (15)  implies  that  if  the 
prediction  mismatch  is  bounded  then  the  stability  is  ensured. 


Remark:  The  right  hand  side  of  (27)  can  be  used  to  define  the  stability  margin  for  the  i- 
th  vehicle  in  general  ease  as  follows: 


Stability  Margin  = 


*  T‘(t,  ) 


;|(i.  ;  )e  £■  { /,  )  r* . 
/*  {It  ) 


(29) 


=  -£'  (t^)  +  V  '  (/j  ) 


The  bigger  stability  margin  the  more  stable  system.  This  definition  of  stability  margin 
will  be  used  in  the  future  sections  to  investigate  the  effectiveness  of  each  method  tor 
enhancing  the  stability. 
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6.5  Improved  algorithms  for  decentralized  RHC 

In  this  section  the  role  of  variable  communication  topology,  variable  prediction  horizon 
and  variable  sampling  time  for  stability  enhancement  is  investigated,  and  finally  an 
algorithm  is  proposed  which  uses  the  results  of  this  section  for  further  enhancement  of 
stability  of  decentralized  RHC. 

Beforehand,  it  is  important  to  find  out  how  the  communication  can  be  performed  to 
enhance  the  stability.  According  to  (29),  for  a  larger  stability  margin  the  integration  of 
mismatch  between  the  predicted  and  actual  trajectories  of  the  neighbours  over  the 
prediction  horizon  must  be  as  small  as  possible.  Hence,  in  order  to  enhance  the  stability 
of  the  system  £•'  should  be  reduced  as  much  as  possible  and  the  communication  must  be 
appropriately  performed  in  this  way.  In  fact,  the  communication  should  be  used  in  such  a 
way  that  the  predicted  trajectory  of  the  neighbour  vehicles  don’t  deviate  too  far  from  the 
actual  values.  Therefore,  in  this  section,  the  communication  techniques  are  utilized  in 
such  a  way  to  reduce  the  mismatch  between  the  predicted  and  actual  trajectories  of 
neighbour  vehicles. 

6.5.1  Variable  Communication  topology 

By  the  expression  “variable  communication  topology”  we  mean  that  the  number  of  the 
neighbours  of  each  vehicle  may  change  during  performing  the  mission;  in  fact,  the  set  of 
neighbour  vehicles  of  each  vehicle  is  time  varying.  The  variation  in  the  number  of 
neighbour  vehicles  of  each  vehicle  is  carried  out  by  two  communication  techniques:  1- 
changing  the  communication  radius  and  2-  multi-hopping  communication.  These  two 
techniques  are  introduced  and  discussed  in  this  section. 


6.5. 1.1  Expanding  the  communication  radius: 

The  communication  radius  defines  a  region  around  each  individual  vehicle  where  the 
individual  vehicle  is  able  to  communicate  with  vehicles  which  arc  inside  this  region. 
Figure  1  shows  this  region  for  a  vehicle  by  a  circle,  the  dashed  circles  show  the  expanded 
communication  radius  and  it  shows  that  with  a  larger  communication  radius  the  vehicle  is 
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able  to  communicate  with  more  other  vehicles.  Hence,  the  larger  communication  radius 
allows  more  neighbours  for  each  vehicle  and  this  drives  the  local  solutions  of  the 
decentralized  RHC  to  be  closer  to  the  global  centralized  solution  which  is  unique  for  all 
vehicles.  This  leads  to  smaller  mismatch  between  predicted  and  actual  trajectories  of 
neighbours,  and  according  to  stability  margin  (29)  this  will  increase  the  stability  margin 
of  the  decentralized  RHC.  Therefore,  generally  speaking  one  can  say  that  a  larger 
communication  radius  can  improve  the  stability  of  the  decentralized  RHC. 

However,  in  the  case  of  larger  communication  radius  the  communication  delay  may 
deteriorate  the  performance  of  the  system.  Also,  for  more  number  of  neighbours,  the 
scale  of  the  decentralized  RHC  problem  is  larger  and  this  leads  to  the  higher  computation 
burden.  Hence,  a  trade-off  between  these  issues  provides  an  optimum  communication 
radius. 


6.5.1.2  Multi-hopping  communication: 

Another  potential  way  for  changing  the  number  of  vehicles  is  multi-hopping 
communication.  From  a  communication  point  of  view,  each  node  (vehicle)  on  the  graph 
is  a  hop  that  can  receive  the  information  from  the  neighbour  nodes  and  transfer  the 
information  to  them.  The  idea  is  to  hop  (transfer)  the  information  from  one  node  of  the 
graph  to  another  until  the  information  arrives  at  the  destination  .  In  fact,  the  information 
is  communicated  between  two  nodes  which  arc  not  necessarily  neighbours. 
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Fig.  6:  Multi-hopping  communication  and  information  flow  between  two  non¬ 
neighbour  vehicles 

Multi-hopping  communication  enables  each  vehicle  to  communicate  with  more  vehicles 
through  its  neighbours.  It  is  true  to  say  that  the  i-th  vehicle  has  access  to  all  vehicles 
which  its  neighbours  have  access  to  them.  Figure  6  shows  how  two  non-neighbouring 
vehicles  can  communicate  through  a  common  neighbour.  The  arrows  show  the  How  of 
information  between  two  non-neighbours  vehicles  communicating  through  a  common 
neighbour. 

Using  multi-hoping  communication,  more  vehicles  are  able  to  communicate  with  each 
other  since  they  are  all  connected  through  multi-hopping.  Hence,  this  will  make  the  set  of 
neighbours  larger  and  consequently  this  allows  the  local  solutions  of  the  decentralized 
RHC  to  be  closer  to  the  global  centralized  solution  which  is  unique  for  all  vehicles.  This 
leads  to  smaller  mismatch  between  predicted  and  actual  trajectories  of  neighbours  and 
hence  according  to  stability  margin  (29)  this  will  increase  the  stability  margin  of  the 
decentralized  RHC.  Therefore,  theoretically,  one  can  say  that  the  multi-hopping  can 
improve  the  stability  of  the  decentralized  RHC.  However,  practically  multi-hopping 
induces  delays  and  noise  that  will  adversely  affect  the  control  algorithm"^.  This  delay  is 
different  for  each  neighbour  and  it  is  seen  in  the  literatures  that  the  effect  of  the  varying 
delay  is  more  deteriorative  than  the  constant  dclay^"*.  Also,  the  effect  of  the  higher 
computation  effort  must  be  considered  like  the  case  of  larger  communication  radius. 
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6.5.2  Variable  Prediction  Horizon 

The  effect  of  the  variable  prediction  horizon  is  investigated  in  this  section.  We  will 
find  some  bounds  on  the  prediction  horizon  so  that  the  stability  is  achieved.  Since  each 
vehicle  uses  a  model  of  the  neighbour  to  predict  the  trajectory  of  neighbour  vehicles,  to 
investigate  the  effect  of  variable  prediction  horizon  on  the  stability  of  decentralized  RHC, 
two  cases  will  be  studied:  1)  model  of  neighbour  vehicle  without  uncertainty  2)  model  of 
neighbour  vehicle  with  model  uncertainty.  Beforehand,  we  make  the  following 
assumption: 

Assumption:  we  assume  that  the  dynamics  of  vehicles  is  Lipchitz  continuous  with 
Lipchitz  constants  L„  and  L,^  as  follows: 

||/(.Y|  ,  M, )  -  f{x^ , )||<  L,  ||.v,  -  .V,  I  +  ||!/|  -  Wj  II  (30) 


6.5.2.1  No  uncertainty  in  the  model  of  neighbour  vehicle 

Suppose  that  p  is  the  mismatch  between  the  predicted  and  actual  control  action  of 
neighbour): 

(31) 


(32) 


Hence,  from  Lipchitz  continuous  assumption,  the  following  hold: 
||/(.v/;',  u,[  ' (t))  -  ,ul-' (t))\\<  pL,, 


Theorem  1:  Sufficient  condition  for  stability  of  the  decentralized  RlIC  architecture  is 
that  every  i-th  vehicle  satisfies  the  following  inequality  at  each  time  step  tp 

)  (33) 


I  , 

+  Nf{T  (/, )  -  S' (t,  ))/l^  (R)<K‘(t, 
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where,  n  is  defined  in  (31),  L  is  lipchilz  constant  of  vehicles  dynamics,  H  c/e'wo/fi-  the 
induced  weighted  norm  of  matrix,  N‘„  is  the  number  of  neighbours  ofi-th  vehicle  and 
^max  the  maximum  eigenvalue  of  matrix. 


Proof:  For  simplicity,  we  first  consider  the  case  of  two  vehicles  {Nv-2),  hence: 


^'=  j  \\xifft)-x;;'{t)tdt+  j  \\ulfft)-u;f‘{t) 


dt 


(34) 


Where, y  is  the  neighbour  of  /,  and  {i=I,j=2).  Also,  from  (31)  we  have: 

U  +  7"  (/,  ) 


(35) 


Using  Bellman-Gronwall  lemma  for  mismatch  (32)  and  taking  integration  yields: 


dt 


(36) 


Where,  Z,,t  is  the  Lipchitz  constant  for  system  (9)  with  respect  to  x.  Some  mathematical 
operations  yield: 


r  II  II  -V  i'k) 

ki.ru,) 

tr 

—  c 

(37) 


Putting  (35)  and  (37)  into  (34)  yields  to  following  bound  on  the and  Using  this 
bound  in  ( 1 5)  yields; 


k 


^JQ) 


(38) 


-p(T{0-dk))2^{R)<K 


The  proof  for  the  general  case  with  Nv  vehicles  follows  along  the  lines  of  the  case  of 
two  vehicles  and  yields  the  stability  condition  (33)  that  defines  a  bound  on  the  prediction 
horizon  T.  Since  the  left  hand  side  of  (33)  is  a  strictly  increasing  function  of  prediction 
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horizon  T,  there  is  a  unique  solution  for  inequality  (33).  Evidently  this  solution  comes 
from  the  communication  limitation  and  hence  is  called  r.  . 

C  omu 


6.5. 2. 2  .Model  of  neighbour  vehicle  with  model  uncertainty 

Assume  that  there  is  the  following  bound  on  the  model  uncertainty: 

1/(^0- (39) 


Theorem  2;  Sufficient  condition  for  stability  of  the  decentralized  RHC  architecture  is 
that  every  i-th  vehicle  satisfies  the  following  inequality: 


(l./ )€£,■'(/*  I 

+  K//(r(f*)-rJ'(/J)^,^,(^)<A-'(/,) 


(40) 


Where,  n  is  defined  in  (31),  pis  model  uncertainty’  of  neighbour  vehicles,  L  is 
Lipchitz  constant  of  vehicles  dynamics,  |||  t/c/rofe.v  the  induced  weighted  norm  of  matrix, 
Nl  is  the  number  of  neighbours  of  i-th  vehicle  and  denotes  the  maximum  eigenvalue 
of  matrix. 

Proof:  An  analysis  similar  to  theorem  1  provides  the  proof  for  this  theorem. 


The  inequalities  (33)  and  (40)  suggest  that  for  stability  of  the  decentralized  RHC  the 
following  must  hold; 

T  <T  (41) 

comu 

In  fact,  the  prediction  horizon  should  be  smaller  than  .  However,  we  should 

consider  that  normally  for  RHC  the  prediction  horizon  should  be  selected  as  large  as 
possible  for  better  performance.  Hence,  selection  of  Tis  a  trade  off  between  stability  and 
performance. 
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6.5.3  Variable  Communication  Rate 


The  communication  sampling  time  is  equal  to  the  execution  time  of  the  RHC.  Also,  it 
is  well  known  that  for  stability  of  RHC  the  execution  time  must  be  smaller  than  the 
prediction  horizon  hence,  it  is  concluded  that  for  stability  the  communication  rate  must 
be  smaller  than  the  prediction  horizon  coming  from  (33)  and  (40).  On  the  other  hand,  it  is 
required  that  each  vehicle  has  enough  time  to  transfer  its  updated  information  to  its 
neighbours.  In  fact,  the  amount  of  communicated  information  mustn’t  exceed  the 
bandwidth  of  the  available  communication  facilities  at  each  sampling  time;  hence,  the 
following  communication  constraint  must  be  respected: 

I  T'*'’ 


<  B 


(42) 


where,  (J!(/)  is  the  sampling  period  of  the  i-th  vehicle  at  time  t,  K,  is  the  amount  of  i-th 

package  (bits/samplc)  and  B  (bits/s)  is  the  bandwidth  of  the  available  communication 
channel.  This  will  impose  a  lower  bound  on  the  communication  rate.  Thus: 

s:.it,)<  S‘(i,)«  T'(ti)  (43) 

The  following  theorem  suggests  that  to  reduce  the  deteriorative  effects  of 
communication  delay  on  the  stability,  the  communication  should  be  performed  with  a 
faster  rate.  Without  losing  the  generality,  the  analysis  is  carried  out  for  linear  systems 
while  the  results  can  be  generalized  to  a  wider  class  of  systems  (the  nonlinear  systems 
can  be  approximated  by  a  linear  system) 


Theorem  3:  Consider  the  optimization  problem  I  with  the  following  linear  deferential 
equation  describing  the  dynamics  of  i-th  vehicle: 

x'(t)  =  Ax‘(t)  +  Bu'(t) 

(4 

x'(/o)  =  x'g  €  x; 


And  also  suppose  that  the  following  provides  the  concatenated  dynamics  of  all 
vehicles  in  the  set  of  neighbours  of  i-th  vehicle: 
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i'it)  =  +  B'lVH) 

-^0 


Then,  the  following  holds  for  each  vehicle  at  each  time  step  ti,: 


x/'fik)  -  xf'ft^)  <  a, 


,  ('*  )  v-  J'J 


Where  a,  and  \  are  positive  numbers. 


(45) 


(46) 


Proof:  the  Hamiltonian  for  the  optimization  problem  1  is  written  as  follows 


25. 


Where  <7  defines  the  vector  of  costacs.  Hence,  the  necessary  condition  for  the 
optimality  is: 

V -  A/  =  0=>  il{t)  =  -{R  +  P  =  -  -R  'B^ P 

2 

Also,  we  have: 

cj  =  -V,A/ 


Combining  this  with  system  dynamics  yields: 


■BR  'B^ 


(47) 


(48) 


(49) 


(50) 


And  for  the  boundary  conditions  we  have: 


<tiT'{t,))  =  V 


ir'tti ) 


(51) 


Solving  the  system  of  differential  equations  (50)  yields  the  following  solutions: 

“(0  =  -  y  )  '  5(fj  )V,“  (0-v(/,  );f  e  [f* -f  r'(^  )] 

•v(0  =  (f  ).v(/*  );^  e  [fj  +  7''(fj )] 

Where, 

(Pu  (0  =  ^^3(0  +  (p^{t).s,^ 

<P,]  (0  =  <Pi(0  +  <Pi(t).s,^ 

Where, 


(52) 


(53) 
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5,^  ^-^-R-'B^W,{T'{t,))-2Pcp,{T'(t,))Y'[lPcp,(T'{t,))-(p,i,T'{t,))] 
And  and  ^C4are  the  components  ofthe  transition  matrix  of  system  (50): 

(p,  ip. 


(54) 


^3(^/o)  = 


(55) 


21 

In  [  ]  it  is  proven  that  the  following  bounds  can  be  defined  on  the  transition  matrix 
components: 

lk(0||<  >  0 


(56) 


If  we  separate  the  inputs  and  states  evolution  equation  for  the  j-th  vehicle  and  shift  that 
in  time  by  t/ steps,  we  will  have: 

2  (57) 


Since  ^  and  ^3,'  obtained  from  the  graph  topology  the  control  action  above  is  a 

function  of  graph  topology.  In  the  decentralized  RlIC  the  j-th  vehicle  transmit  this  control 
action  to  its  neighbour  vehicle  in  order  to  the  neighbours  use  this  control  action  for 
prediction  of  x,'  ' ;  in  fact,  the  i-th  vehicle  uses  this  control  action  over  the  horizon 

Ih-d^h]  while  the  j-th  vehicle  use  this  control  action  during  [f*_j,/j_^/^i]and  at  the  next 
time  step  the  j-th  vehicle  uses  the  updated  control  trajectory  i//  '  (f);  hence,  this 
mismatch  between  what  j-th  vehicle  plans  and  what  i-th  vehicle  assumes  about  that 
induces  a  delay  induces  a  mismatch  between  x/ '  and  x/  '.  Therefore,  since  .v/  ' and 

'i  ^  'i 

x,'''are  used  to  solve  optimization  problems  P'(/^)and  P'(/^)it  is  required  to  find  the 
mismatch  between  .v/'^  and  .v/  '  as  follows: 

'k  'i 


(58) 


Also: 
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(59) 


Since  is  transmitted  to  i-th  vehicle  and  it  is  received  by  i-th  vehicle  at 

time/^ ,  then  jr'  .  Subtraction  (58)  and  (59),  taking  nonn  and  using 

inequality  (56)  yield; 


K'  (^)  -  X,['\  (f;  )||=  ,(/*  )<P',  ,  (^.,)....^P,;  (^-,/,,)  -  (PI  ,  (^)1||1-V"'(/*.J 

<  ll.r {t^_ 


(60) 


Where  a,  and  are  positive  numbers. 


Remark:  inequality  (60)  says  that  a  larger  sampling  timed''(fj),  will  increase  the 

mismatch  between  x''^(t^)  and  x^'{t^)  which  are  the  initial  condition  of  both  f"(fj)and 

P'(/j)  ,  and  this  will  lead  to  mismatch  between  the  trajectories  that  P'(fj)and 

P' (/*)  generate  forj-th  vehicle.  Then,  decreasing  of  the  sampling  time  (f* )  which  means 

a  faster  communication  rate  will  improve  the  feasibility  and  stability  of  the  decentralized 
RllC. 


6.5.4  Proposed  Algorithm  for  communication  topology  optimization 


In  this  section  based  on  the  results  of  the  previous  section  a  new  improved  algorithm  for 
decentralized  RHC  is  proposed.  First  a  sub-algorithm  is  proposed  which  allows  using  the 
maximum  available  communication  capacity  for  choosing  the  best  combination  of 
communication  topology,  communication  rate  and  prediction  horizon  so  that  the  stability 
enhancement  of  decentralized  RHC  is  achieved.  Then,  this  sub-algorithm  is  merged  with 
the  decentralized  RHC  algorithm.  Once  for  any  reason  the  performance  and  stability  of 
decentralized  RHC  architecture  is  not  reasonable,  the  new  communication  based 
approach  can  be  used  to  enhance  the  stability.  This  approach  consists  of  three  main  steps: 
1-  the  communication  topology  is  changed  for  enhancement  of  stability  based  on  the 
results  of  section  5.1.  2-  Based  on  the  new  communication  topology  the  graph  topology 
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(section  3)  are  updated.  3-  The  communication  rate  and  prediction  horizon  are  selected 
for  the  new  problem  so  that  the  stability  is  guaranteed  (section  5.2  &  5.3).  The  new  sub¬ 
algorithm  is  proposed  in  figure  7: 


Fig.  7:  Sub-algorithm  for  optimization  of  graph  topology  and  decentralized  RHC 

architecture 

Figure  7  shows  an  algorithm  which  optimizes  the  graph  topology  and  decentralized  RHC 
parameters  in  order  to  use  maximum  communication  capacity  for  improving  the  stability 
and  performance.  More  specifically  the  output  of  this  algorithm  is  the  optimum 
communication  topology,  communication  rate  (execution  time)  and  prediction  horizon.  In 
this  diagram  u  is  a  positive  small  number;  a  bigger u  leads  to  a  more  conservative  design 
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which  doesn't  use  maximum  available  communication  topology.  At  each  sampling  time 
each  vehicle  uses  this  algorithm  to  find  the  best  communication  topology,  execution 
horizon  (sampling  time)  and  predietion  horizon.  It  is  important  to  mention  that  this 
selection  is  restricted  to  available  communication  capability  and  the  decentralized  RllC 
stability  requirements.  We  call  this  sub-algorithm  as  “Communication  Topology 
Optimization  Algorithm”  (CTOA);  we  combine  the  CTOA  with  the  decentralized  RllC 
algorithm  as  follows: 


Modified  Decentralized  RHC  Algorithm: 

8)  Let  k=0. 

9)  Compute  optimized  graph  topology  £"(/*) ,  communication  rate  and 

prediction  horizon  by  “Communication  Topology  Optimization 
Algorithm”  (CTOA). 

10)  Sample  x'„„,,(c). 

11)  Communicate  neighbour  vehicles  and 

receive  <,„„,(/*  -  ))  where  (/,  J)  e  E‘  (/* ) . 

12)  Solve  the  decentralized  optimization  problem  PiOk)  cwt/ generate  the  control 
action  for  time  interval  [thtk'^T(tk)]- 

13)  Execute  the  control  action  for  individual  vehicle  over  the  time  interval  [f*, 
4+/]- 

»'(/)  = »;;(/);  /  6  (61) 

14)  ^=A'-i-/.  Goto  step  2. 


This  modified  decentralized  RIIC  algorithm  allows  using  the  maximum  available 
communication  topology  for  stability  and  performance  enhancement. 
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6.6  Example 


6.6.1  Case  Study  I:  Formation  of  a  group  of  mobile  robots 


The  simulation  example  involves  the  formation  stabilization  of  a  group  of  five 
differentially  driven  wheeled  mobile  robots,  where  it  is  desirable  that  each  robot  takes  a 
predefined  position  and  finally  reaches  an  assigned  target.  Using  feedback  linearization 
the  equations  of  a  mobile  robot  can  be  transformed  into  a  two  dimensional  double 

'>6  ”>7  ,, 

integrator  “  as  lollows: 

.V,  =  x. 

X’)  =  Ui 

r  _  (62) 

Xy  -  .<-4 

^4  = 


where,  (.v^Xj) denotes  the  position  vector,  {x2,x^}  is  the  set  of  velocity  components,  zr/ 


and  U2  arc  the  inputs  to  the  robot.  These  dynamics  can  also  serve  as  a  description  for  a 
rotorcraft-like  UAV  flying  at  constant  altitude.  Random  noise  is  added  to  the  dynamics 
to  investigate  the  effect  of  uncertainty.  The  following  undirected  graph  topology  is  used 
to  study  the  formation  stabilization  of  a  group  of  five  robots; 


V  =  11,2,3,4,5} 

E  =  {(1,2),(2,3),(3,4),(3,5)} 


(63) 


All  matrix  penalties  in  the  cost  function  are  taken  as  the  identity  matrix  except  the 
components  corresponding  to  the  velocities  are  multiplied  by  0.001.  B-splinc  basis 
functions  are  used  to  generate  the  desired  trajectories.  Furthermore,  the  concept  of  flat 
outputs  **  is  utilized  to  reduce  the  dimension  of  the  optimization  problem  and  alleviate  the 
online  computation  burden.  The  reader  is  referred  to  [11]  for  a  comprehensive  review  on 
the  trajectory  generation  of  the  optimization  problems. 

A  “task  allocator  unit”  is  also  developed  that  minimizes  an  overall  cost  to  go  for  all 
vehicles  and  assigns  one  target  to  each  vehicle.  In  fact,  all  the  possible  cases 
(permutations)  of  vehicles  to  targets  are  considered  and  a  globally  optimal  permutation  is 
chosen. 

The  performance  is  measured  by  calculating  the  final  cost  of  entire  group;  obviously, 
it  is  desired  to  minimize  the  final  cost  for  better  performance.  The  optimization  toolbox 
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of  Matlab  is  used  for  solving  the  optimization  problem.  Form  stability  condition  (40), 
for  5  robots  is  approximated  correspondingly  as  follows  2.5,  2.5,  3.5,  2.5,  2.5  that 
implies  for  any  prediction  horizon  smaller  than  these  values  the  decentralized  RHC 
problem  must  be  stable.  To  verify  that,  simulation  results  for  pair  (r,^)  =  (1,0.2)  (where  T 
<  Tcomu)  (all  time  units  are  sec.)  is  depicted  in  figure  8,  this  figure  shows  the  formation  of 
a  group  of  5  robots  aiming  to  visit  their  assigned  targets  marked  by  5  stars  on  the  right 
part  of  figure,  the  initial  position  of  vehicles  is  shown  by  5  circles  on  figure  8.  As  it  is 
seen  the  robots  reach  their  final  position  precisely;  however,  figure  9  shows  that  if  we 
increase  the  prediction  horizon  and  reduce  the  communication  rate  (or  increase  the 
execution  time)  to (r,J)  =  (5,0.3)  (where  T  >  Tcomu),  the  robots  don’t  reach  their  assigned 
target;  hence,  the  feasibility  is  not  achieved  that  verifies  the  results  of  theorem  2. 
However,  the  stability  can  be  improved  in  this  case  by  aiding  the  results  of  section  IV. B 
(or  IV-C)  through  changing  the  communication  topology;  in  fact,  the  communication 
radius  is  enlarged  to  allow  more  neighbour  for  each  robot,  the  new  graph  topology  is 
presented  as  follows: 

E  =  {(1,2),(2,3),(2,4),(3,4),(3,5),(4,5)}  (64) 

Figure  10  shows  the  simulation  results  for  this  enhanced  graph  topology.  As  is  it  seen 
the  performance  of  the  entire  group  is  improved  by  this  new  graph  topology. 

The  codes  related  to  the  simulation  of  this  case  are  stored  in  the  folder 
“Deccntralized_tasklO”  and  the  results  can  be  compared  with  the  centralized  case  stored 
in  folder  “Centralized  task  10”. 
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Fig.  8:  Formation  of  a  group  of  robots  with  (7,  J)  =  (1,0,2) 


Fig.  9:  Formation  of  a  group  of  robots  with  bigger  prediction  horizon  and  execution 

time:  (7',(y)  =  (5,0.3) 


Fig.  10:  Formation  of  a  group  of  robots  with  larger  communication  radius  and 
bigger  prediction  horizon  and  execution  time:  (I,  J)  =  (5,0.3) 
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6.6.2  Case  Study  II:  Formation  of  a  Fleet  of  UAVs 

In  this  case  a  flight  formation  of  a  fleet  of  rotorcraft  UAVs  with  the  following  3DOF 
dynamics  is  considered  [^]: 


r~2  ~  ^rnr. 

u  =  vr  +  c^ii\lii  +v - ' 

m 

V  =  -ur  +  c^.vylu^  +v^ 

[j/  =  r 

=t/cos(^//')-vsin(^//) 

=  u  sin(^//)  +  vcos(^//) 

Where: 

rn 

* 

Cy=- 

I'm  ’ 

Where; 

=0.1 

Pa  =  \-^kglm^ 
m  =^.2kg 
'k„.,.^  =-0.3 
/,,=0.91 

=0.2^kgxnr 


(65) 


(66) 


(67) 
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It  is  desired  to  investigate  a  case  when  the  vehicles  leave  the  formation  due  to  obstacles 
(or  far  targets)  (see  figure  1 1),  and  consequently  they  need  a  variable  communication 
topology.  In  fact,  the  following  topologies  will  be  used  during  performing  the  mission: 


V  =  {1,2,3} 

E(fJ  =  {(1,2),(1,3),(2,3)} 
^(^)  =  {(2,3)} 

£(/,)=  {(1,2),(1,3),(2, 3)} 


(68) 


This  variable  graph  topology  shows  that  UAVs  first  fly  in  formation  (  E(l„) );  then  they 
leave  the  formation  to  avoid  obstacle  (£(/,))  and  finally  they  again  return  to  the 
formation  ( E{t^)).  This  graph  topology  allows  studying  a  time-varying  communication 
topology. 


®  Target  position 


UAV 


^  T rajectory  of  UAVs 


®  Obstacle 


V 

UAVs  leave  the  formation  in 
order  to  visit  the  targets;  they 
make  smaller  subgroups 


UAVs  return  to  the  formation 
after  visiting  the  targets 


Fig,  11:  Formation  of  a  fleet  of  UAVs:  a  time  varying  communication  topology  due 

to  obstacle 


The  parameters  of  the  problem  are  listed  below: 
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r  =  1 ,0  sec  . 

S  =  0.05  sec  . 

Rc  =  6  m 
Rs  =  2  m 
0  <  <  1000 
'  0  <  r,,  <  20 

^max  =  ^  0  ml  sec  (Speed  C  onstraint) 
Q  =t//ag(0,l,0.1,0.1,0.1,20,20) 

R  =  cy;ag(0.02,0.02) 

=cy/flg(i, 1,1, 1,1,1) 


(69) 


These  values  are  used  for  all  the  vehicles.  The  forward  velocity  u  and  yaw  angle  if/wW 
be  used  as  the  flat  outputs.  These  flat  outputs  are  parameterized  as  a  function  of  time  by 
the  means  of  cubic  Spline  functions  [^®].  RllCOOL  library  along  with  SNOPT 
optimization  package  [^'j  will  be  used  to  solve  this  problem.  The  obstacle  is  modeled  as  a 
circle  and  the  vehicles  are  not  allowed  to  collide  that.  Mathematically,  this  restriction  is 
implemented  by  the  following  constraint: 

x;(0+>'c(0>/?L  =  i6  (70) 


The  trajectory  of  all  vehicles  must  respect  this  constraint  during  performing  the  mission. 
The  SNOPT  optimization  package  allows  non-convex  optimization  problem  with 
nonlinear  constraints;  hence  applying  the  above  nonlinear  constraint  is  possible  by 
SNOPT.  Figure  (12)  shows  the  trajectories  of  this  scenario: 
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Fig.  12:  flight  formation  of  a  fleet  of  UAVs:  a  time  varying  communication  topology 

due  to  obstacle 

As  it  is  seen  from  the  figure  (12)  the  UAVs  visit  their  assigned  target  and  avoid  the 
obstacle  (non-fly  zone)  and  collision.  Figure  (13)  shows  the  distance  of  the  vehicle  at 
each  time  step: 
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t,  Sec 


Fig.  13:  The  distance  between  vehicles  during  performing  the  mission 

As  it  is  seen  from  the  above  figure  the  distance  between  UAVl  and  the  other  two  UAVs 
exceed  the  communication  radius;  hence,  for  some  time  interval  the  UAVl  is  not  the 
neighbour  of  the  other  two  UAVs  and  change  the  graph  topology.  Also,  as  it  is  seen  the 
UAVs  keep  the  safe  distance  (Rs)  and  don’t  contact  each  other. 

The  codes  related  to  the  simulation  of  this  case  are  stored  in  the  folder 
“RllCOOL  Decentralized  task  10”. 
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6.7  Conclusion 

The  effect  of  variable  communication  on  stability  of  decentralized  RHC  of  multi-vehicle 
systems  has  been  investigated.  Since  each  vehicle  uses  a  model  of  neighbours  to  predict 
their  trajectory,  the  effect  of  model  uncertainty  on  the  stability  of  the  entire  group  is 
investigated  and  bounds  are  found  on  the  prediction  horizon  and  communication  rate  that 
guarantee  the  stability.  The  analysis  shows  that  faster  communication  rates  and  smaller 
prediction  horizons  can  improve  the  stability  of  decentralized  RHC.  Furthermore,  it  is 
seen  that  expanding  the  communication  radius  and  using  the  multi-hopping 
communication  can  improve  the  stability  of  decentralized  RHC.  A  modified 
decentralized  RHC  algorithm  is  proposed  which  allows  using  the  maximum  available 
communication  capacity  for  stability  enhancement  of  decentralized  RHC.  Two  different 
case  studies  have  been  investigated  to  clarify  the  problem  statement.  Future  work 
includes  solving  some  sophisticated  case  studies  with  varying  communication  topology 
and  varying  communication  rates  to  verify  the  theoretical  results  of  this  report. 
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Abstract 

This  report  documents  the  work  related  to  tasks  #11  for  the  project  entitled 
“Synthesis  and  Implementation  of  Single  -  and  Multi-vehicle  Systems  Guidance 
Based  on  Nonlinear  Control  and  Optimization  Techniques”.  This  report 
describes  the  design  of  a  real  time  scheduling  algorithm  for  computation  and 
communication  scheduling  of  multiple  uncertain  receding  horizon  control 
systems,  running  on  multiple  distributed  computers.  In  this  work,  execution 
horizon  and  communication  period  of  all  systems  are  determined  such  that 
directly  optimize  robust  RHC  performance  of  the  whole  network.  This  is 
achieved  through  minimization  of  the  upper  bound  on  the  so-called  overall 
closed-loop  cost  of  the  system,  subject  to  scheduling  constraints  on  both 
computation  and  communication.  The  performance  of  the  method  is 
demonstrated  by  simulating  it  in  Visual  C++  by  using  RHC  Object  Oriented 
Library  (RHCOOL),  which  is  adapted  for  Dynamic  Scheduling  of  Decentralized 
RHC  Systems. 
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7.1  Introduction 

Scheduling  of  RHC  computational  and  communication  is  very  important  for 
decentralized  RHC  control  since  it  allows  optimization  of  resources  to  achieve  maximum 
performance  in  the  presence  of  uncertainty,  disturbances,  and  delays.  The  tasks  addressed 
in  this  report  related  to  the  project  entitled  “Real-time  Scheduling  of  Multiple  Uncertain 
Receding  Horizon  Control  Systems”  is  as  follows: 

Task  1 1:  Design  a  scalable,  dynamic  run-time  scheduling  algorithm  for  multiple  RHC 
running  on  distributed  digital  processors. 


In  this  report,  we  consider  the  problem  of  controlling  multiple  uncertain  nonlinear 
systems  by  means  of  concurrent  RHC  schemes.  The  systems  considered  in  this  report  are 
dynamically  decoupled.  However,  because  of  being  in  a  network,  the  coupling  term  is 
appeared  in  the  controller  cost  function.  In  addition,  the  proposed  formulation  is  adapted 
for  decentralized  application,  so  the  communication  delay  is  also  accounted  for  in  this 
report. 

A  new  scheduling  approach  is  proposed  by  combining  the  results  from  continuous  time 
nonlinear  systems  theory  and  the  concept  of  Rate  Monotonic  Priority  Assignment  (RM) 
[3].  Assuming  the  prediction  horizon  is  a  known  constant  and  using  a  single  computing 
resource,  the  new  technique  determines  the  execution  horizons  of  all  RHC  systems.  The 
execution  horizon  determination  of  each  subsystem  while  optimizing  the  performance  is 
cast  into  a  constrained  optimization  problem.  The  robust  performance  is  formulated  in  the 
objective  function  and  the  schedulability  condition  is  guarantied  using  a  constraint. 
Online  solution  of  the  foresaid  optimization  problem,  using  the  updated  optimization 
parameters,  results  in  dynamically  determining  the  execution  horizons.  This  report  can  be 
regarded  as  the  first  systematic  approach  for  dynamic  scheduling  of  multiple  RHC 
systems  subject  to  computing  and  communicating  resource  limitations  and  model 
uncertainties. 

The  report  is  organized  as  follows: 

Chapter  2  presents  the  dynamic  scheduling  of  decoupled  RHC  systems  using  a  single 
processor.  It  is  supposed  that  the  systems  have  no  data  exchanging  and  they  are 
completely  decoupled.  In  this  chapter,  after  problem  formulation  and  scheduling 
formulations,  a  discussion  is  presented  for  performance  improvement  by  using  a  new 
upper  bound  instead  of  Gronwall-Bellman  Lemma  [4]. 

In  Chapter  3,  the  systems  are  considered  to  be  coupled  in  their  controller  cost  functions. 
The  scheduling  algorithm  presented  in  this  section  is  applicable  to  the  coupled  systems 
on  a  single  processor. 

Chapter  4  is  the  general  case,  the  communication  delay  is  also  added,  and  a  new  method 
for  communication  scheduling  is  presented.  The  scheduling  algorithm  proposed  in  this 
section  is  using  the  results  of  previous  sections  and  is  applicable  for  scheduling  of 
multiple  RHC  systems  on  distributed  processors.  In  this  method,  computation  and 
communication  scheduling  is  formulated  by  optimally  determining  the  execution  horizon 
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and  communication  period  of  all  systems  in  the  network.  Summary  and  future  works  are 
presented  in  Chapter  5  and  the  references  are  in  Chapter  6. 


7.2  Scheduling  of  Multiple  RHC  Systems  without  Coupling  on  a  Single 
Processor 

Receding  horizon  control  (RHC)  is  a  repeated  online  solution  of  a  finite  horizon  open- 
loop  optimal  control  problem  [1],  Based  on  the  currently  available  state  values,  the 
optimal  control  problem  is  solved  and  the  resulting  control  input  is  applied  to  the  plant  in 
a  period  called  the  execution  horizon.  In  real-time  implementation  of  a  RHC  problem,  the 
execution  horizon  is  the  closed-loop  sampling  period.  It  should  be  selected  carefully  to 
obtain  a  suitable  trade-off  between  computational  expense  and  controller  performance. 

Application  of  RHC  to  control  problems  with  multiple  subsystems  such  as  multiple 
Unmanned  Aerial  Vehicle  (UAV)  systems  can  be  addressed  by  applying  RHC  to  the 
individual  subsystems.  This  results  in  multiple  RHC  processes  that  must  be  scheduled  in 
an  appropriate  manner  to  achieve  optimal  performance  in  the  presence  of  computing 
resource  limitations.  Systematic  methods  for  scheduling  RHC  systems  are  typically  not 
discussed  in  the  literature,  despite  the  fact  that  computational  delays  resulting  from 
scheduling  can  dramatically  affect  stability  and  perfonnance.  A  dynamic  scheduling 
approach  for  multiple  discrete-time  decoupled  linear  RHC  systems  has  been  previously 
developed  [5].  This  method  assigns  the  priorities  to  different  subsystems  based  on  the 
value  of  a  computational  delay  dependent  cost  index.  Premature  termination  of  the 
optimization  process  is  employed.  However,  in  [5],  uncertainties  in  the  subsystems  are 
not  explicitly  accounted  for.  The  authors  in  [6]  considered  the  problem  of  optimal  on-line 
assignment  of  sampling  periods  for  a  set  of  linear-quadratic  (LQ)  controllers.  They  used 
feedback  from  the  plant  states  to  distribute  the  computing  resources  optimally  among  the 
tasks. 

In  this  chapter,  we  consider  the  problem  of  controlling  n  decoupled  uncertain  nonlinear 
systems  by  means  of  concurrent  RHC  schemes.  A  new  scheduling  approach  is  proposed 
by  combining  the  results  from  continuous  time  nonlinear  systems  theory  and  the  concept 
of  Rate  Monotonic  Priority  Assignment  (RM)  [3].  Assuming  the  prediction  horizon  is  a 
known  constant  and  using  a  single  computing  resource,  the  new  technique  determines  the 
execution  horizons  of  all  RHC  systems.  The  execution  horizon  detennination  of  each 
subsystem  while  optimizing  the  performance  is  cast  into  a  constrained  optimization 
problem.  The  robust  performance  is  formulated  in  the  objective  function  and  the 
schedulability  condition  is  guarantied  using  a  constraint.  Online  solution  of  the  foresaid 
optimization  problem,  using  the  updated  optimization  parameters,  results  in  dynamically 
determining  the  execution  horizons. 
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7.2.1  Problem  Statement 

7.2. 1.1  RHC  formulation 

Consider  the  following  nominal  equation: 

x  =  f(x(f),u(/),0  (1) 

which  serves  as  a  model  for  the  actual  system  described  by: 

X  =  f(x(/),u(0,/)+g(x,u,/)  (2) 

where  \{t)edV  and  x(/)e'Jl"  are  the  nominal  and  actual  states  of  the  system, 
respectively.  The  input  vector  u(7)Gt)l'”  satisfies  the  constraints  u{l)eU  (V/>0), 
where  U  is  the  allowable  set  of  inputs.  Furthermore,  it  is  assumed  that  (A1-A3)  in  [2]  are 
also  satisfied;  that  is,  f  is  twice  differentiable,  U  is  compact  and  convex,  and  system  (1) 
has  a  unique  solution  for  a  given  initial  condition.  The  finite  horizon  cost  (from  initial 
state  x(/))  is  defined  as  follows  [7]: 

7,(x(/),u(-))=  J|'"^^(x“(r;x{f)),u(r))t/r  +  K(x“(r  +  r;x(/)))  (3) 

where  T  is  the  optimization  horizon  of  the  RHC  controller.  The  optimal  cost  is  given  by: 

J’(x(0)  =  inf  J7.(x(/),u())  (4) 

U{.) 

The  optimized  trajectory  resulting  from  (4)  is  defined  as 
(x*  (r;x(7)),u^(r;x(/))) ,  r  e  (f,/ -f  T] .  In  the  closed  loop  RHC  the  calculated  input 
uj-(r;x(/))  is  applied  to  the  actual  system  (2),  and  t  e{l,l  +  S],  while  S  is  called  the 
Execution  Horizon  {5  <T). 

7.2. 1.2  Real-time  scheduling  of  multiple  RHC  systems 

Consider  the  problem  of  controlling  n  decoupled  uncertain  nonlinear  systems  using  the 
RHC  scheme.  The  following  equations  serve  as  the  nominal  model  for  describing  the 
decoupled  plants 

X,.  =f,(x,(0,U;(/),/),  /  =  !,...,«  (5) 

in  addition,  the  actual  plants  are  described  by 

X,  =f,(x,(0,u,(OT)+g,(x,,u,,0,  /-1,...,«  (6) 


7-5 


Chapter  7:  Real-time  Dynamic  Scheduling  Algorithm  for  Multiple  RHC  Running  on  Distributed  Digital  Processors 


For  RHC  control  of  a  single  apparatus  using  a  single  computer  the  calculated  inputs  are 
applied  to  the  system  for  a  period  equal  to  the  execution  horizon  of  the  system. 
Therefore,  in  real-time  implementation,  a  common  way  is  to  define  a  real-time  periodic 
task  [3],  with  its  period  equal  to  the  execution  horizon  of  the  system. 

Suppose  that  the  systems  described  in  (5)  and  (6)  are  connected  to  a  single  computer  for 
feedback  control.  From  a  computer  control  point  of  view,  each  control  system  can  be 
handled  as  a  periodic  task  in  the  real-time  programming.  The  period  of  each  periodic  task 
is  equal  to  the  execution  horizon  of  its  related  subsystem.  Determining  these  periods  (or 
execution  horizons)  is  not  trivial.  In  this  report,  a  systematic  approach  is  presented  to 
calculate  these  periods. 

For  the  proposed  approach,  the  execution  horizons  of  all  subsystems  should  be  defined 
such  that  the  overall  performance  of  the  system  is  maximized.  In  order  to  evaluate  the 
performance  of  the  system,  the  following  cost  function  is  proposed  as  the  cost  of  the 
closed  loop  system  from  time  t  to  /  -i-  T,,  where  Tj  is  the  period  that  calculated  execution 
horizons  would  be  applied  to 


V 


(7) 


where  +  ))  optimal  input  applied  to 

subsystem  /. 

n 

The  idea  is  to  find  the  execution  horizon  of  each  subsystem  ( S, ),  such  that 

1=1 

minimum. 

The  scheduling  algorithm  that  is  presented  in  this  report  is  based  on  the  concept  of  Rate 
Monotonic  Priority  Assignment  (RM)  [3].  The  system  is  schedulable  using  RM,  for  a  set 
of  n  tasks,  if  the  following  inequality  is  valid  [3]: 

"  S  - 

jLJ  =  y  —  <n(2"-\)  (8) 

,=1  P, 


where  //  is  CPU  Utilization  factor,  is  the  computation  time  of  subsystem  /,  and  is 
the  period  of  task  /,  which  is  equal  to  the  execution  horizon  of  subsystem  i  {p,=  <5, ). 

Remark  1:  In  this  report,  we  propose  a  dynamic  scheduling  algorithm,  which  updates  the 
execution  horizons  for  the  period  of  time  T2  >  S/,  so  we  force  the  proposed  scheduling 
method  to  be  valid  under  the  condition  of  (8)  and  it  guarantees  schedulability  of  the 
system. 


32 


Floor  function  of  a  real  number  .v,  denoted  L-vJ,  is  the  largest  integer  less  than  or  equal 


to  .V. 
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7.2.2  Model  Based  Dynamic  Scheduling 


Equation  (7)  needs  the  future  states  of  the  actual  systems  and  the  future  optimized  inputs 
(from  time  t  to  t +  T.,  )  for  calculating  J,  which  arc  not  available  at  current  time  (t). 

Therefore,  instead  of  calculating  ,  the  following  cost  is  proposed  which  is  an 
estimation  of  the  cost  in  (7): 


(x(0,u;(.))  =  Z  y  f '  ^/,(x,  (r),u;  ,  (r;x,.(f)))  dr 


(9) 


Proposition! : 

Suppose  the  following  assumptions  hold  true: 

1.  q,  is  quadratic,  so:  9.(x,.,u  J  =  x' 0,x, -t- uf /?,u, 

2-  =  xf^^x,.  is  Lipschitz  continuous  with  constant  EJ . 

3.  f;  in  equation  (5)  is  Lipschitz  continuous  with  constant  L. . 

4.  g,  in  equation  (6)  is  bounded  and ||g,(X;,u,.)||  <  6,  . 

Then  the  following  inequality  exists: 


f  ‘''^/(x,(r),u;  ,(r;x,(f)))c/r  < 

P"'  ^*(xr,,(z';  x,(/)).  u;,,(r;x;(f)))  dT  +  ^ 

V 


xMio) 


)) 


where  ,  is  the  bound  on  measurement  errors  caused  by  sensor  noise. 


Proof: 

From  assumption  1 : 


From  Lipschitz  continuity  of0, : 


^  =  pfe(x,(r))-a(x,(r)))c/r  <  F:||x,.(r)- x,(r^^/r 


(12) 
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Using  the  Gronwall-Bellman  inequality  [4]; 


A< 


x,(0-x,(/)l|  + 


(r-/) 


(13) 


The  term  ||x^(0- x,(/)||  denotes  the  error  in  state  measurement.  It  is  assumed  that  the 
measurement  noise  is  the  only  error  source  with  the  bound  6, ,  such  that 

||x,(0-Xi(0|l^^,  (14) 


Therefore,  from  (13)  and  (14): 


A<P. 

\ 

P  (  ! 

\  (  1  /  \  V 

J 

dT  =  ^\  h^\e 

-l)+6  —  -1  -d', 

'1  ^  -  l/ 

/ 

) 

^  W  ’  Jj 

(15) 


So,  from  ( 1 1 ),  ( 1 2),  and  (15): 
f"'''^,(x.(r),u;,(r;x,(f)))c/r 


1)4- 

(16) 


vA 


In  addition,u*r,(.)  =  u‘^,(r;x-(/))  so: 

pa(x,(r;xXf))Vr-f  p(u;,(.)f /?,u;,(.)t/r  ['"'9,(x;,(r;x,(f)),u;,(r;x,(/))Vr 

(17) 


which  is  a  part  of  optimal  open  loop  cost,  calculated  at  time  t.  Combination  of  (16)  and 
(17)  result  in  (10). □ 


Proposition  2: 

Consider  n  decoupled  systems  with  equations  presented  in  (5)  and  (6),  controlled  by  RHC 
using  only  one  computer.  In  addition,  the  assumptions  of  proposition  I  arc  valid.  Since 
uncertainties  in  the  subsystems  are  different  and  the  measurements  arc  performed  with 
bounded  sensor  noise,  show  that  the  following  equation  can  be  used  to  optimally 
determine  the  execution  horizons  of  all  subsystems: 
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min  V  — 


+  ■ 


p 

/ 

\  f  ]  / 

w 

K 

JJ 

JJ  J 


(18) 


s.t. 


^Ib.i  ^ub.i 


where  and  are  the  maximum  and  minimum  acceptable  execution  horizons  for 
subsystem  /,  respectively.  is  the  execution  horizon  of  subsystem  /  before  being 
updated. 


Proof: 

From  (9)  and  result  of  proposition  1,  the  following  inequality  is  valid: 


4.(x(0,Ur(.))< 


M  ^',V 


v4 


(19) 


where  as  explained  in  (9)  the  left  hand  side  is  the  estimation  of  overall  cost  function 
presented  in  (7).  Therefore,  by  minimizing  the  left  hand  side,  the  performance  of  the 
system  improves.  In  addition,  as  explained  in  Remark  1,  equation  (8)  must  hold  to 
guarantee  schcdulability  of  the  system  using  RM  method.  Therefore,  it  is  a 
straightforward  to  propose  the  following  for  scheduling  algorithm,  considering  a,  as 
weights  applied  in  each  subsystem: 


f 

f 

'''  (x r., (r;  X, (/ )),  u; ,  (r ;  X, (/ )))c/r  +  ^ [  ft, ,  (e 

«-i)+i, 

f  1 

V 

^  ))) 

s.t.  < 


5  - 

4.,  <  ^  ^ubj 


(20) 


However,  the  term  |  '9,(x^,(r;x,(?)),u^,(r;x,(7)))r/r  cannot  be  calculated  directly, 

since  it  needs  the  optimal  inputs  and  states  for  time  t,  but  the  execution  horizons  must  be 
defined  before  time  t.  So,  the  following  term  is  proposed  instead: 
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,  (x r./  (z-;  X,  (/  -  , )),  u ; ,  (r ;  X,  (f  -  )))f/ r 


that  uses  the  optimal  states  and  inputs  from  previous  open-loop  cost  calculation. 
Combination  of  (20)  and  (2 1 )  completes  the  proof  □ 


7.2.3  Dynamic  Scheduling  Algorithm 

In  the  previous  section,  a  preliminary  cost  function  was  presented  in  (18)  which  can 
predict  the  execution  horizons  of  all  subsystems,  based  on  the  limited  available 
computational  resource.  In  this  section,  the  updated  form  of  cost  function  is  presented 
which  considers  the  delay  because  of  computation  time.  Furthenuore,  an  algorithm  is 
presented  to  update  the  scheduling  parameters. 

Before  proposing  the  algorithm,  the  delay  caused  by  computation  should  be  handled. 
Based  on  the  method  proposed  in  [9],  for  controlling  one  system  by  the  RHC  method,  the 
computation  is  performed  in  the  period  of  time  Mof  -f  <5,  and  the  result  is  applied  to  the 
period  of  t  +  5  {ot  +  25 .  For  the  dynamic  scheduling  of  multiple  systems,  we  propose 
the  following  method  by  considering  the  computational  time  of  each  subsystem.  Consider 
Figure  1,  which  illustrates  the  method  for  two  subsystems. 

Let  us  consider  time  t  as  the  time  that  the  scheduler  starts  again.  As  shown  in  this  figure, 
t[  is  the  time  that  first  subsystem  was  started  its  last  computation  and  is  the  same  as  the 
time  that  the  last  sampled  data  of  subsystem  1  became  available.  Consequently,  /f  is  the 
similar  time  for  subsystem  2.  The  computation  of  scheduler  starts  right  after  completing 
computation  of  both  subsystems.  Therefore,  the  scheduler  can  be  considered  as  a  periodic 
function  with  a  fixed  period  Tj  and  the  lowest  priority  comparing  to  the  periodic 
functions  of  all  subsystems. 

Remark  2.  T2  must  be  selected  such  that  the  execution  horizons  of  all  subsystems  remain 
less  than  T2,\.e.  <  Tt  ;  \<i<n.  In  addition,  the  closed-loop  cost  in  the  interval  of  I  to 

t  +  T2  (equation  (7))  has  been  estimated  linearly  with  the  interval  Mo  t  +  5,  in  equation 
(9).  Therefore,  T,  should  be  selected  such  that  not  being  far  larger  than^, . 
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Subsystem  | 
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1  1 

O/  — •  6/  - 

I 

p  0,  - 

start  new  5| 


Subsyslem 


CPU  Usage 
Subsyslem  I 


i 

start  new'  6, 


CPU  Usage 
Subsyslem  2 


Scheduler  Trigged 

New  Execution  Horizons  arc  computed 

Figure  1-  Schematic  diagram  illustrating  the  dynamic  scheduling  procedure 


Time 


Time 


Time 


Time 


If  the  computation  of  Scheduler  optimization  problem  is  considerable,  the  execution 
horizon  of  the  last  computed  subsystem  (before  trigging  scheduler)  can  be  increased  by 
while  /„  is  the  upper  bound  in  computational  time  needed  to  solve  scheduler 

optimization  problem. 


Based  on  the  foresaid  explanations  the  modified  scheduling  optimization  problem  can  be 
expressed  as  follows: 


9,  '.  ('r  I  (r;  S|  (',"  ))(/'■ 


min  >  — 


(22) 


S.t.  ( 


y  ^<a7(2''  -1) 

^ihj  -  ^uhj 


Remark  3:  This  algorithm  must  be  updated  for  every  Tj  while  the  states  must  remain  in  a 
specified  region  (box)  |x,  R,  ,  where  x^, ,  defines  the  center  and  R,  presents  the 

dimensions  of  the  box.  It  must  be  noted  that  the  scheduling  parameters,  P,,  and  I,  are 
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functions  of  states,  so  Pj  =  -  />,(x^ ,),  and  for  the  predefined  R, . 

The  procedure  for  dynamic  scheduling  can  be  expressed  as  follows: 

1.  Based  on  the  eurrent  states  and  predefined  R; ,  create  a  domain,  in  which  the 
states  will  remain  for  the  subsequent  period  s  • 

2.  Calculate/;  =  /;(x,.,),  =  ^,(x,.,)  and/,  =  /((x^,).  See  section  (IV-B) 

3.  Estimate  ,  from  previous  computational  time.  Sec  section  (IV-A) 

4.  Solve  equation  (22)  and  calculate  for  all  subsystems. 

5.  Update  the  next  execution  horizons  by  the  calculated  Sj  from  step  4. 

6.  Wait  for  the  next  seconds  and  repeal  the  procedure  from  step  1 . 

Computation  time  estimation 

Different  methods  can  be  used  to  estimate  less  eonservative  upper  bound  for  computation 
time.  Since,  this  issue  is  not  the  focus  of  this  report,  a  simple  method  is  used  which  uses 
the  previous  to  estimate  the  new  one.  Based  on  this  method,  the  maximum  from 

time  1-/3  to  fis  considered  as  the  basis  of  upper  bound  calculation  for  from  lime  t 

lot  +  T2.  Tj  should  be  defined  in  the  design  process. 


7.2.4  Discussion  on  Performance 

In  this  section,  the  proposed  dynamic  scheduling  algorithm  is  brielly  reviewed  and  the 
validity  and  performance  of  the  method  is  discussed.  This  method  was  originated  from 
the  following  three  main  steps; 


1 .  An  execution  horizon  dependent  cost  function  was  proposed  in  (7).  This  cost 
function  was  estimated  by  another  eost  function  and  estimated  in  (9).  The  last 
cost  was  used  as  the  main  objective  for  minimization  in  the  rest  of  the 
algorithm. 

2.  The  upper  bound  of  that  eost  index  was  found  in  Proposition  1 . 

3.  For  the  dynamic  scheduling  algorithm,  we  minimized  the  upper  bound  and 
claimed  that  this  minimization  results  in  the  minimization  of  the  cost  function 
presented  in  (9). 


Fiowever,  the  claim  made  in  the  final  step  is  valid  if  the  proposed  bound  (step  2)  is  close 
enough  to  the  actual  value  of  (9).  In  other  form,  if  the  proposed  upper  bound  is  far  away 
from  the  actual  value  of  (9),  then  minimization  of  the  bound  does  not  affect  the  actual 
value  of  (9),  i.c.,  it  does  not  affect  the  performance. 

In  the  proof  of  Proposition  1,  the  term  ||x,(/ +  c>,  )  -  x,(/ +  (5',  )||  which  is  the  state  estimation 
error,  was  replaced  by  its  upper  bound  presented  in  [4],  which  is  a  result  of  the  Gronwall- 
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Bellman  (GB)  Lemma.  For  a  speeific  problem  with  a  distinct  nonlinear  nominal  function 
and  bounded  unmodcled  dynamics,  finding  a  less  conservative  bound  is  possible.  In  this 
section,  the  foresaid  bound  is  calculated  for  some  systems  and  compared  to  the  bound 
results  from  GB  Lemma. 

Later  on,  an  algorithm  is  developed  to  estimate  the  upper  bound  as  a  function  ofcJ, , 
offline.  Afterwards,  in  online  programming,  the  parameters  of  is  updated  based  on  the 
response  of  the  system,  and  used  for  dynamic  scheduling. 

Before  digging  into  the  examples,  the  problem  formulation  is  presented. 


7. 2. 4.1  Problem  formulation 

Suppose  that  an  identified  model  of  the  physical  (actual)  system  is  available.  This  model 
is  called  actual  model  (refer  to  equation  (2)).  This  model  can  have  bounded  uncertainties 
in  parameters.  In  addition,  a  simplified  control  oriented  model,  which  we  call  it  nominal 
model  is  also  available  (refer  to  equation  (1)).  The  goal  is  to  find  as  a  function  of  5 

{\x.,J\{5) )  such  that 
||x(/-b^)-x(f  4- j)||</;,(^) 

where  x(/)  is  available  by  measurement.  Two  cases  are  considered  in  this  study: 

1  -  If  there  is  no  error  in  measurement,  then  ||x(/)  -  x(f  )||  =  0 

2-  Ifbounded  noise  exists  in  measurement,  then||x(/)- x(/)||  <  ^)^.(x),  where  is  the 

bound  on  sensor  noise  and  it  can  be  dependent  on  the  state  of  the  system  in 
general. 

Note  that  is  a  function  of  5  and  states  (i. c. ,  //,(<?,  x) ).  First  we  suppose  that  4  's  only 
a  function  of  d  and  we  find  J],{5)  such  that  it  is  valid  for  the  entire  acceptable  state 
space.  After  that,  the  parameters  of  that  function  arc  updated  in  online  programming, 
using  the  measurement  data.  Therefore,  finding  j\  casts  into  two  problems: 

Problem  1: 

Finding  the  form  of  bound  function  i.e.,  to  determine  that  if  J\,(d)  is  an 

exponential  function  or  it  is  a  second  order  polynomial  or  any  other  forms.  This  task  is 
done  offline.  f^{5)  can  be  found  by 

•  numerical  simulations 

•  analytically  by  solving  the  math  problem,  for  every  special  system 

In  this  report,  the  Van  dcr  Pol  oscillator  is  simulated  as  an  example  and  a  polynomial  is 
proposed  instead  of  Gronwall-Bellman  Lemma  (GB). 
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Problem  2: 

Updating  the  parameters  of online.  For  example,  if  is  the  same  as  the  bound 
presented  by  GB,  then  b  and  L  should  be  found  online  and  used  in  dynamic  scheduling. 

One  way  to  find  (update)  the  parameters  online,  is  using  the  previous  data  for 

estimation.  is  a  function  of  5  or  time  and  it  is  an  upper  bound  on||x(d')-x((>')||. 

Therefore,  for  estimating  the  parameters  of/^(<5'),  the  term  ||x(<:^)- x(r>')||  should  be 
known  for  different  values  of  J  . 

In  this  report,  the  example  is  solved  by  having  an  offline  lookup  table.  In  the  examples 
that  will  be  presented  in  the  next  report,  the  online  estimation  of  parameters  will  be  used. 

7.2.4.2  V  an  Der  Pol  Oscillator  example 

For  example,  consider  a  Van  dcr  Pol  Oscillator  with  the  following  equations: 

f -v,  =  -v,  [  2  <  .v,  <  5 

[.v,=(l-.r,-).v,-.v,  l-2<.v,<2 

[x2  =  (1  +  a,  )(l-.r|-).r2-(l +  <:/,).{-,  [Ifljl^O.lS 

This  system  is  simulated  using  MATLAB  and  in  every  the  maximum  value 
of||x(J)-x(^)||,  callcd||x(<!))-x(<J)||^^^ ,  and  the  estimated  bound  by  Gronwall-Bellman 

Lemma,  is  plotted  in  Figure  2.  ||x(J)- x(£))||^^^^  is  obtained  in  every  S  by  changing  x(0) 
in  all  over  the  acceptable  domain,  expressed  in  (23),  while  x(0)  =  x(0) . 
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Figure  2-  Different  estate  estimation  error  bounds  vs.  estimation  time  where  no  noise  is 
eonsidered  in  measurements  for  van  Der  Pol  Oseillator  example,  expressed  in  (23)  and  (24). 


As  shown  in  Figure  2,  the  difference  between  the  actual  bound  and  GB  bound  increases 
with  larger  estimation  time.  The  example  clearly  illustrates  the  necessity  for  using  other 
bounds  in  akin  problems.  In  Figure  3  a  linear  upper  bound  is  presented.  Using 
polynomials  of  higher  order,  sometimes  results  in  having  a  better  estimation,  while 
adding  the  complexity  of  the  approach. 
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Figure  3-  A  linear  function  is  used  as  upper  bound  for||x(c) )  —  x((5)||^^^^ ,  instead  of  upper  bound 

presented  by  BG  Lemma. 
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7.3  Scheduling  of  Multiple  Systems  with  Coupling  on  a  Single 
Processor 

Definition  1.  The  set  /t,  is  called  the  neighbouring  set  of  subsystem  /,  and  consists  of 
any  subsystem  in  the  network  that  has  direct  interconnection  with  the  subsystem  /. 

Assumption  1.  Let  us  consider  the  following  inequality: 

fx, (f  +  J)-  X,. (/  +  ^), / II  <  5,  (cJ, X, (/),  X,  (f ))  (25) 

This  means  that  by  knowing  the  actual  and  nominal  states  at  time  t  (i.e.,  x,(/)  andx,(f)), 
the  error  in  estimating  the  norm  of  state  variables  of  subsystem  /  is  less  than  fl,  at 
timef  +  5 . 

Remark  4.  As  explained  in  Chapter  7.2,  Section  7.2.4,  in  equation  (25)  is  a  function  of 

6  with  the  state  dependent  parameters.  These  parameters  are  updated  at  the  beginning  of 
the  scheduling  algorithm  and  do  not  change  during  the  operation  of  scheduling  algorithm 
for  finding  new  execution  horizons  (periods).  Therefore,  without  loss  of  generality  we 
consider  5,  as  a  function  of  (5,  i.e.  Bj{S)  in  the  remaining  parts  of  this  report. 


Problem  Formulation 

Consider  a  network  of  n  dynamically  decoupled  subsystems  using  the  RHC  approach  in  a 
decentralized  manner.  The  cost  function  associated  to  each  subsystem  can  be  expressed 
as  follows: 


J,((,7',x;,u,,x,)=  1^' 


^,(x,(r,/),uXr))+ 


VIt 


(26) 


where  ^,(x,(r,/),u,(r))=  x[(r,/)0,x,.(r,f)-i- u^(r)/?,u,(r)  and  x,  is  a  vector  containing 
the  states  of  all  neighbouring  subsystems  of  the  subsystem,  is  a  function  which 
defines  the  interaction  between  two  nodes  of  the  system  (network)  x,  andx^  . 


Remark  5.  For  simplicity  of  the  proof,  it  supposed  that  the  interaction  function  is  just 
between  two  nodes  of  the  system,  i.e.  =g^(x,,xj.  After  that,  the  formulation  is 
generalized  for  any  combinations. 
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As  an  extension  to  the  (9),  the  following  equation  is  proposed  as  an  estimation  of  the 
elosed  loop  eost  in  the  existenee  of  eoupling  between  subsystems. 


i=l  i  I 


(21) 


Lemma  1.  Consider  the  following  assumptions: 

1-  If  X;  and  x^  are  two  wxl  column  vectors,  x,^  is  a  2a?; xl  column  vector  such 

thatx,^  =[xT,x/f  . 

2-  Let  g/t(xy)  be  Lipschitz  continuous  with  constant/.^,  and  gk[\,j)  be  a  positive 
scalar. 

3-  From  Assumption  1,  we  can  have:  ||x,  (/ -t- <5>)- x,  (f -i- d')j|  <  and 

\\\j{t  +  S)-Xj{t  +  S'\\<Bj{s) 

Show  that: 

\gk  (x/  (/  +  ,  ('  +  ^))-  gk  k  ('  +  +  ^  )  (28) 


Proof: 

From  Lipschitz  continuity  of  g^.  we  have: 

|g*  (xy  (/  +  (>'))-  gk  [t  +  rl)  ))|  <  I  x^.  (r  +  d)-  x^.  (/  -h  S 

Furthermore  (by  omitting  (f  +  <^)  for  simplicity): 

||Xy  -  x,||  -  [x,"  ,  X  I  -  [x/  ,  x/  f  II  =  [(x,  -  X,  Y  >  (x  /  -  X  J  f  II  = 

[(x,  -  X,  f ,  (x^  -  x^  f  ]|  <  ||(x,  -  X,  f  II  +  ||(x ,  -  X  J  II  =  ||(x,  -  X,  I  +  ||{x  -  X  ^ 


(29) 


(30) 


Using  the  third  assumption  of  this  Lemma,  results  in: 


X,;  -  Xy  < 


(31) 


Combination  of  (29)  and  (31)  results  in  (28)  which  completes  the  proof  □ 


Corollary  1.  Combination  of  Proposition  1  and  Lemma  1,  results  in  the  following 
inequality: 
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P  (x/(r).U/,,(z-;x, (/)))+ 


JeA, 


L/r  < 


(32) 


Note  that  the  result  of  corollary  1  is  important  since  it  can  be  used  to  propose  an  upper 
bound  on  the  closed  loop  cost  of  (27). 


In  Proposition  2,  the  term(7,(x^,(r;x,(f)),U;-,(r;x,(f)))  was  estimated  by  using  the  optimal 
states  and  inputs  resulted  from  the  last  open-loop  cost  calculation  and  resulted 
inry,(xr,.(r;x,(f-cJp,)),u*-,(r;X;(/-(5^  J)).  Similarly,  the  term  gj.,.)  can  be  estimated 
using  the  most  recent  calculation  done  on  x,  andx^  .  Therefore,  the  following 

optimization  problem  is  proposed  for  scheduling  of  n  dynamically  decoupled  subsystems 
in  an  interconnected  network,  on  a  single  processor: 


mm 


i"! 


1=1 


f  p+b-, 


[^i  (* r,  (^;  X /  (/  -  )}  u  ; ,  (r;  x -  5 )))  +  /^  (r  -  f 

Sk  (x  r ,,  (z-;  X  ,•  (/  -  dp  . )),  X  ].  j  (r;  x  ^  (f  -  5 p  ^ ))) 


C  f 


_  f  ' 
5  .  •* 


Za 


je.Ai 


dr 


(33) 


s.t. 


X^<«(2"  -1) 

dib.,  <d.<  d„^, , 


uh,i 


If  B-G  lemma  is  used  for  finding 5,,  and  noise  in  measurement  is  considered  (like 
Proposition  1),  we  would  have: 

(34) 

Therefore,  the  scheduling  optimization  problem  can  be  expressed  as: 
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S.t. 


y  -1) 

^IbJ  <  -  ^ub.i 


(35) 


Remark  6.  The  formulation,  which  is  presented  here,  is  compatible  for  decentralized 
application,  since  each  node  has  a  special  cost  function.  Extension  of  the  present 
formulation  is  done  in  the  next  chapter,  for  the  cases  of  having  more  than  one  processor, 
sharing  the  information  via  a  communication  network  in  the  presence  of  communication 
delay. 

Discussion 

The  extension  of  the  proposed  method  can  easily  be  done  for  having  the  general  case  of 
coupling  between  the  nodes  of  the  network.  The  key  point  in  the  presented  method  is  the 
proof  of  Lemma  1,  which  has  two  specifications: 

1-  The  Lipschitz  continuity  of  gf.  ',  this  specification  is  not  depend  on  the  number  of 
nodes,  i.e.  for  a  function  gi^  which  is  depend  on  more  than  two  nodes  of  the 

system,  the  Lipschitz  continuity  can  be  defined  without  any  problem. 

2-  The  second  specification,  which  is  used  in  the  proof  (equation  (30))  is  actually  a 
general  rule  that  the  norm  of  a  vector  is  not  larger  than  the  summation  of  norms  oj 
its  sub-vectors.  Therefore,  this  specification  is  valid  for  the  general  case  of  g*  as 
well. 

Therefore,  the  proposed  method  is  general  enough  for  dynamic  scheduling  of  multiple 
systems  having  interconnection  in  their  controller  cost  function,  on  a  single  processor. 
For  example,  the  formation  of  multiple  vehicles  using  a  single  processor  is  one  of  the 
cases  that  this  method  can  be  applied. 
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In  addition,  the  case  that  is  studied  in  this  chapter  is  important  because  as  stated  before, 
by  generalization  of  this  method,  the  case  of  multiple  subsystems  on  multiple  computers 
can  be  done.  This  case  is  studied  in  the  next  chapter. 
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7.4  Computation  and  Communication  Scheduling  of  Multiple  Systems 
on  Multiple  Distributed  Processors 

The  problem  under  study  is  illustrated  in  Figure  4.  For  this  case,  we  consider  delay  in 
communication  and  we  determine  execution  horizon  and  communication  period  for  each 
subsystem. 


Processor  1  Processor  2 


Computation 


Communications 


Processor  3 


Figure  4-  Illustration  of  Computation  and  Communication  Scheduling  for  the  case  of  three 
processors  and  presented  configuration. 


7.4.1  Problem  Statement 

Suppose  having  two  computers  connected  via  a  local  network  and  these  computers  have 
n^  and  number  of  subsystems  using  RllC  approach,  respectively.  The  total  number  of 
systems  is  n^,.  Therefore,  we  have:  = /7| -i- /?2  .  The  method  is  later  generalized  for 

more  than  two  computers. 

Assumption  2:  Communication  period  should  be  where  is  the 

communication  period  of  system  /,  S,  is  the  execution  horizon,  and  z,  is  an  integer. 

Remark  7:  Maximum  communication  delay  of  the  data  sent  from  node  i  to  node  j,  is 
equal  to: 

=Ci  +^ij 

where  (^,y  is  the  delay  because  of  senders  and  receivers  and  this  can  be  considered  as  a 
fixed  (known)  delay  if  the  hopping  between  nodes  /  and  j  does  not  change. 


7-21 


Chapter  7:  Real-time  Dynamic  Scheduling  Algorithm  for  Multiple  RHC  Running  on  Distnbutcd  Digital  Processors 


It  is  supposed  that  the  data  sent  from  subsystem  j,  has  delay  equal  to  r,y ,  when  it  is 

received  in  subsystem  i.  Therefore,  the  open-loop  cost  of  subsystem  /,  can  be  expressed 
as  follows; 


y,(r,7',x,.,u,.,x,.)=  I"' 


Like  equation  (9)  in  chapter  7.2,  we  propose  the  following  cost  as  an  estimation  of  the 
closed  loop  cost: 


^  A 


'='v"'  V  7  j 


(37) 


Similar  to  what  we  have  done  in  previous  chapters,  we  arc  going  to  find  an  upper  bound 
on  equation  37.  Before  considering  this,  we  present  a  Lemma  which  considers  the  effect 
of  communication  delay. 

Lemma  2.  Consider  the  following  assumptions: 

1-  If  X,  and  x^  arc  two  wxl  column  vectors,  x,^  is  a  2«?xl  column  vector  such 

that  x^.  =  [x,.^,x/f  . 

2-  Let  >’*(xy  )=g;t(x^, x^)  be  Lipschitz  continuous  with  constant  ,  and  v^(x,y)  be  a 
positive  scalar. 

3-  From  Assumption  1  and  Remark  4,  we  can  have:  ||x,(r -i- r!>)- x,(/ -i- r!>,f)j|  <  (<>'). 

4-  The  data  which  is  received  in  node  /  from  node y,  has  a  delay  ofr^  . 

Show  that: 

(x,  X  y  (/  -f  (5))<  (x,.  (/  +  J,  / ),  X ^  (r  +  (5,  f  -  r,y  ))-i-  (fi,  (cJ )+  (j  +  r,^ )) (38) 


Proof: 

From  Lipschitz  continuity  of  we  have: 


Vk  (x,;  ('  +  <?> ))-  T*  f  [x,  ( r ,  X ,  (f  +  rJ,  /  -  )  J 


(f  +  ri) )-  [x,. (/  +  ri',  /  7  ,  x^  (/  +  Sj-  J  ] 


(39) 


Asx  <  a:  ,  we  can  have: 
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yk' 


(xy  (/  +  >'*  [  [x,  (/  +  ,  X,  (t+S,t-  C,J  J  f 


+ 


f{t+s)- [x,(/ +(?,//,  (/ + y  ] 


Furthermore: 


Xy(/  +  ^)-  [x,(/  +  s,t)!  ,  Xj  (t  +  y  f  II  < 

||x,.(r  +  S)-  x,.(/  +  r!),f  ]|  +  ||x^  (f  +  S )-  \j  (t  +  S,t-  4"^ 
Using  the  third  assumption  of  this  Lemma,  results  in: 


(40) 


(41) 


(42) 


Therefore,  combination  of  (41)  and  (42)  results  in: 

x,y (( +  s)-  \^{t  +  S,t)  , \j  (/  +  J, /  - C,j  y  f  I  <  B, {s)+  Bj (c>  +  )  (43 ) 


Combination  of  (40)  and  (43)  results  in  (38)  which  completes  the  proof  □ 


Corollary  2.  Similar  to  what  we  expressed  in  Corollary  1,  combination  of  Proposition  I 
and  Lemma  2,  results  in  the  following  inequality  in  presence  of  communication  delay: 


I 

(•?/  (’‘*7-3  (^;  X/  (' ))’  “  r.i  (^;  X,  (/)))+  F;  5,  (r  -  7 ))/ 

^*(x7-,/{cXy(()),x*.^(r;x^(; 
^+L^{fi,(r-7)+5^(r-7-brJ) 


< 


(44) 


r"  I 


Vir 


where,  xj  ^(r;Xy(/^))  indicates  the  optimal  trajectory  of  subsystem  y,  resulted  from 
sampling  point  (See  Appendix  A  to  have  better  understanding  about  communication 
delay  and  t^).  In  addition,  we  have  two  cases  for  r,^  (See  Assumption  2,  and  Remark 

ly. 

f  Tjj  =  Sj  ,  if  i  and  j  are  in  the  same  computer 
I  Ty  =  ZjSj  +  ,  if  i  and  j  are  in  different  computers 
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Remark  8:  It  should  be  noted  that  for  decentralized  application,  this  is  valid  for  the  case 
that  we  are  using  the  produced  trajectory  by  the  neighbours,  i.c.,  in  order  to  find  optimal 
inputs  of  subsystem  /,  the  optimal  trajectories  that  have  been  estimated  by  the  neighbours 
are  considered  [13]. 

Based  on  the  result  of  Corollary  2,  the  following  optimization  problem  is  proposed  for 
computation  and  communication  scheduling  of  n^,  subsystems  on  two  computers,  by 
having  the  assumption  that  :  (computation  scheduling  means  finding  the  proper 

values  for  S,  and  the  communication  scheduling  means  finding  the  proper 
communication  rate  (or  period)  for  each  subsystem.  The  combination  of  these  two 
problems  leads  to  finding  S,  andz, .) 


min-( 


^cpu\ 


1=1 

"I  +«2 


=>CPU2 

i-ti,  -f  1 


sJA 


-1)  (y  =  l,2) 


R 

y  ^<Bw 


(45) 


where  is  the  amount  of  Bytes  to  be  sent  in  each  communication  of  system  /,  and  BIV 
is  the  part  of  network  Bandwidth  that  we  want  to  use.  G,  is  defined  from  the  following 
(same  as  the  previous  scheduling  cost): 


G.  = 


Y  ( fc-.'  ))’  “  r ^  '  )V  r 

YLXBXr-t)-^B{z-t^z^6Y4ij\ 


r«,r 

1  V 

f 

1 

(f  +  S, 

+ — 

f 

1 

V 

dr 


(46) 


If  B-G  lemma  is  used  for  finding  5,  ,  and  noise  in  measurement  is  considered  (like 
Proposition  1),  we  would  have: 

(47) 
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Combination  of  (46)  and  (47)  by  assuming  no  noise  in  measurement  (i.e.,/7^ ,  =  0 ), 

results  in: 


f  ( 
a 


G.  = 


f (x*/-,! »*7M (c X,  (0))+ 


V  V 


f  L.S,  , 
'  e  ‘  '  -I 


-S. 


\  \ 


dr 


JJ 


^  Sk  (xy-.i  (2-;  X,  (/ )),  x]-  ^  (r;  x ^  (t )))cy r 


L..b.  f  e‘-^'  -  I 


s  ' 


V 


V  A 


-S, 


+ 


(  L,[z, 6, 


L, 


JJ 


(48) 


Remark  9;  It  should  be  noted  that  in  the  formulation  of  (45)  it  is  supposed  that  any 
subsystem  needs  to  communicate  with  other  processor,  i.e.,  it  has  a  neighbour  on  that 
processor. 

Remark  10:  The  term  BW  is  equation  (45),  is  a  part  of  the  available  bandwidth  and  we 
cannot  use  the  whole  bandwidth  of  the  network.  Similar  to  the  criteria  on  the  usage  of 
CPU,  when  we  use  Rate  Monotonic  Priority,  it  is  possible  to  find  the  criteria  on 
communication.  However,  in  the  case  of  communication,  we  deal  with  non-preemptive 
tasks  and  some  assumptions  should  be  considered  in  the  length  of  messages. 


7.4.2  Extension  to  More  than  Two  Processors 

Generalization  of  the  approach  to  the  cases  of  having  more  than  two  processors  is  done  in 
the  following. 

Extension  of  the  algorithm  is  almost  straightforw'ard  except  the  constraint  on  the 
communication  bandwidth.  Sec  Figure  (4)  as  an  example  of  more  than  two  processors.  In 
this  example,  subsystems  1,  2,  and  3  are  in  processor  I,  subsystems  4  and  5  are  in 
processor  2,  and  subsystems  6,  7,  8,  and  9  are  in  processor  3.  If  the  neighbours  of 
subsystem  2  for  example  are  1,  7,  and  8,  then  the  information  of  subsystem  2  should  be 
sent  to  processor  3  only.  This  example,  help  us  in  the  following  definition. 

Definition  2:  C,  is  the  set  of  processors  that  have  at  least  one  neighbour  of  subsystem  /. 
Note  that  the  processor  having  subsystem  /  on  it,  is  not  considered  in  C, . 

Using  the  definition  ofC,  ,  the  constraint  on  communication  bandwidth  can  be  defined  as 
follows: 


(49) 
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7.5  Application  to  Hovercraft  Problem 

In  this  Section,  the  simulation  results  of  applying  proposed  scheduling  method  in 
Section  7.2,  on  multiple  hovercraft  systems  is  investigated. 

For  the  hovercraft  example,  the  upper  bound  presented  by  Gronwall-Bellman  (GB),  was 
close  to  the  upper  bound  resulted  from  the  actual  system  simulation.  Therefore,  the  bound 
that  is  used  in  this  example,  is  GB  and  for  dynamic  scheduling,  the  parameters  in  GB  {h 
and  L)  should  be  updated. 


7.5.1  Hovercraft  Model 

A  complete  discussion  on  the  modeling  of  the  RC  hovercraft  is  presented  in  [9].  Since 
only  viscous  friction  is  considered  in  the  nominal  model,  the  model  is  similar  to  that 
presented  in  [10].  From  the  results  of  [9],  it  is  straightforward  to  assume  a  linear 
relationship  between  the  applied  voltage  and  the  thrust  produced  by  the  propellers. 
Therefore,  based  on  Figure  5,  the  nominal  model  expressed  in  local  coordinate  is  given 
as: 


=  +«/^/  u)+vr 

M 

v  =  — v-»r  (50) 

M  ^ 

where  M  is  the  mass,  J  is  the  moment  of  inertia  about  the  Z-axis,  and  b^ ,  6,  and  arc  the 
coefficients  of  viscous  friction  in  X,  Y,  and  rotational  directions,  respectively.  Also, 
and  V,  are  the  applied  voltages  to  the  right  and  left  propeller  DC  motors,  respectively. 

Another  model  of  the  RC  hovercraft  with  more  details  is  considered  as  the  actual  model 
of  the  system  and  used  in  simulations.  This  model  is  as  follows  [9]: 
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^p.i  = 


j 


-k,,C0p,U))-k^^c0p/ -k,,co^,,(u-^r) 
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a  V 


-  k,^(o p  ,{t))-  k^yco pj~  -  k^,(o p  ,{u  +  ^r) 

K  I  ) 


u  = 


j_ 

M 


( 


I 


V  = 


V 

III 

M 


ka\('^p/  ->^o2(^p.r  »-T'-  -^k^,^(Op,-  -k^jCOp,  U  + -P  -  f  f  „ 


I 


■  gsin((^)  -  ur 


I 


2J 


// 

f  / 

f 

f 

1 

-  — r 
2 

ka\^pj  ~  kol^^p.l 

kcl\^^p.r~  ~  k ^2^^ p,r 

u  - 

\\ 

^  ^  JJ 

K 

\ 

Uj 

-gsin(f?)  vr 


III 

j 


(51) 


where  is  the  moment  of  inertia  of  propeller  and  rotating  part  of  the  DC  motor, 
and  cOpi  are  the  angular  speed  of  the  right  and  left  propellers,  respectively.  0  and  ^  arc 
the  pitch  and  roll  angels  of  hovercraft  indicating  the  ground  slop.  J) and  /^ ,  arc 
the  friction  functions  presented  in  [9]. 

This  model  includes  actuator  dynamics,  propeller  dynamics,  effect  of  hovercraft  speed  on 
the  thrust  produced  by  propeller,  nonlincaritics  in  friction  applied  in  hovercraft,  and  the 
ground  inclination.  In  addition,  noises  in  measurements  arc  applied  in  simulations. 


Figure  5-  RC  hovercraft  model  with  produced  thrusts  by  propellers  and  local  (body)  and  global 

coordinate  systems 


7,5.2  Simulation  Results 

The  proposed  scheduling  algorithm  in  chapter  7.2  is  applied  in  the  following  simulation. 
This  is  done  by  applying  the  method  two  hovercrafts.  These  hovcrcrafts  arc  following 
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similar  circular  paths,  and  have  different  levels  of  uncertainty.  Subsystem  2  is  the 
hovercraft  with  more  uncertainty  in  its  model.  Two  cases  are  considered:  static 
scheduling  which  the  execution  horizons  of  two  systems  does  not  change  during  process, 
and  dynamic  scheduling  using  the  proposed  cost  function  and  algorithm.  The  simulations 
are  performed  using  an  Intel  Pentium  IV  processor  with  2.66  GHz  speed,  Microsoft 
Visual  C++  6.0,  Venturcom  RTX  6.0.1,  and  the  RHC  Object  Oriented  Library 
(RHCOOL)”. 

For  the  RC  hovercraft  example  with  model  parameters  presented  in  [11],  the  Gronwall- 
Bellman  bound  is  close  to  the  system  response  [11].  Therefore,  the  cost  function 
presented  in  (18)  is  used  in  this  example.  In  addition,  the  scheduling  parameters  (F*  ,6, 

andL; )  are  found  offline  for  different  values  of  states.  Since  the  state  variables  u,  v,  and  r 

are  the  only  variables  that  have  effects  on  the  scheduling  parameters,  only  these  states  arc 
used  to  find  the  scheduling  parameters.  These  parameters  arc  stored  in  different  tables 
and  arc  used  in  on-line  simulations. 

The  paths  followed  by  two  systems  in  static  scheduling  and  dynamic  scheduling  cases  arc 
presented  in  Figure  6  and  Figure  7,  respectively.  In  the  first  case,  the  periods  of  two 
subsystems  arc  selected  based  on  the  worst-case  computation  time  and  are  0.4  and  0.31 
seconds  for  subsystem  1  and  2,  respectively. 

As  illustrated  in  these  two  figures,  in  dynamic  scheduling  case,  both  hovcrcrafts  were 
able  to  follow  their  trajectories  better  than  the  static  scheduling  case. 
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Figure  6-  Paths  followed  by  two  hovercrafts  in  static  scheduling.  Execution  horizon  of  Subsystem 
1  is  0.4  second  and  for  Subsystem  2  is  0.31  seconds.  The  bolded  lines  arc  representing  the 
position  of  the  center  of  each  hovercraft. 


”  This  library  was  developed  recently  in  the  CIS  Lab,  Concordia  University.  For  more  information,  see 
[12],  Section  3.3. 
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Figure  7-  Paths  followed  by  two  hovererafts  in  dynamie  seheduling  using  the  proposed  algorithm 


In  order  to  have  better  feeling  about  the  performance  of  the  presented  cases,  the  position 
errors  of  both  subsystems  regarding  their  reference  trajectories  arc  demonstrated  in 
Figure  8  and  Figure  9. 

6,  .  -  -----  -  - ^ - , - -  - 

—  Subsystem  1 


Figure  8-  Error  in  trajectory  following  for  both  subsystems  in  static  scheduling  case 
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Figure  9-  Error  in  trajectory  following  for  both  subsystems  in  dynamic  scheduling  case 


Figure  10  shows  the  changing  in  execution  horizons  of  both  subsystems  in  dynamic 
scheduling  case.  Since,  Tj  was  selected  1  second  in  this  case;  the  execution  horizons 
remain  unchanged  for  about  1  second.  In  addition,  the  results  arc  presented  in  shorter 
period  for  more  clarity  of  the  diagram. 


0.45 


-•“Subsystem  1 
-A  ■  Subsystem  2 


Figure  10-  Changing  of  execution  horizon  vs.  time  in  dynamic  scheduling  ease 
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7.6  Conclusion  and  Future  Works 

In  this  report,  a  new  method  was  developed  for  dynamic  scheduling  of  multiple  RHC 
systems.  The  problems  studied  in  this  report,  have  nonlinear  dynamics  subject  to 
uncertainties  in  the  model  and  sensor  noise.  The  formulation  presented  in  this  report  was 
developed  based  on  a  bound  from  the  Gronwall-Bellman  Lemma.  This  lemma  calculates 
a  conservative  bound  in  some  nonlinear  problems  and  should  be  replaced  by  less 
conservative  bounds  in  highly  nonlinear  systems.  The  proposed  algorithm  was  applied  to 
multiple  RC  hovcrcrafts  simulations  to  illustrate  the  new  approach. 

This  report  was  explaining  Task  #1 1  of  the  first  proposal. 
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7.8  Appendix  A:  A  Discussion  on  Communication  Delay 

The  delay  term  r-j  whieh  eonsidered  in  (36)  can  vary  from  to  -i-  where  is  the 

delay  in  sender  and  receiver,  and  4”,  is  the  communication  period  of  subsystem  /'.  Since  in 

the  scheduling  algorithm,  we  find  an  upper  bound  and  optimize  that  bound,  the  maximum 
value  of  that  delay  is  considered  in  the  scheduling  optimization  problem  (45).  In  the 
following,  this  varying  delay  is  described. 

Consider  Figure  1.  The  data,  which  are  receiving  in  subsystem  i  from  subsystem  j,  can 
arrive  at  any  time  and  two  ultimate  cases  arc  shown  in  Figure  1-b.  Suppose  having  no 
computation  delay. 

Case  I)  If  the  data  arrives  just  a  little  before  sampling  time  t  (f  -£■ ),  the  updated  data  of/ 
will  be  used  in  calculation  of  subsystem  i.  Therefore,  the  data  from  /  have  a  delay  equal 
io^i^+£.  Therefore,  when  £  goes  to  zero  (£^->0),  we  have  the  least  delay,  i.e. 

X  j  (r;  l  -  )  and  it  is  the  most  lucky  case. 

Case  2)  The  second  case  is  when  the  data  is  arrived  Just  a  little  after  sampling  time  t 
(/-(-£•).  In  this  case,  last  available  data  from  j  will  be  used  in  /.  Therefore,  when  £■ ->  0 
we  have  x^(r;f  -  -4ij)  which  is  the  worst  case. 

In  summary,  in  the  case  that  we  have  communication  delay,  but  no  computation  delay  we 
can  express  the  trajectory  of  as  x^(r;f -r,y)  where: 

[  tj  -  6j  ,  if  i  and  j  are  in  the  same  computer 

|r^  =ZjSj  ,  if  i  and  j  are  in  different  computers 

Remark  1 1 : 

The  term  4^^  -^,^i  is  Just  the  communication  period  and  even  if  we  are  simulating  on 

one  computer,  this  term  can  occur,  however  in  the  case  of  having  J  and  i  on  the  same 
computer,  Zy  =  1 .  In  addition.  It  is  for  the  case  that  we  arc  using  the  produced  trajectory 

by  the  neighbours  (i.e.,  subsystem  j),  while  we  arc  calculating  the  inputs  and  trajectories 
of  system  /. 
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Subsystem 

/ 


c=- 

Time 


Subsystem 

/ 

6, 

- 

6, 

1  1  Time 

Arrival  of  data 
from  subsystem 
/ 


(a) 


i  i 

t 

t-&  /+c 

(b) 


Figure  1-  part  (a)  schematically  illustrates  two  subsystems  with  different  execution 
horizons  having  communication.  Part  (b)  indicates  two  ultimate  eases,  Lucky  ease  (t  -  e) 

and  worst  ease  ( f  +  f ) 
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Abstract 

This  report  documents  the  work  related  to  tasks  #12,  #13,  and  #14  for  the  project  entitled 
“Synthesis  and  Implementation  of  Single  -  and  Multi-vehicle  Systems  Guidance  Based 
on  Nonlinear  Control  and  Optimization  Techniques”.  This  report  brictly  describes  the 
design  of  a  real  time  scheduling  algorithm  for  computation  and  communication 
scheduling  of  multiple  uncertain  receding  horizon  control  systems,  running  on  multiple 
distributed  computers.  Verification  of  the  proposed  method  has  been  performed  using  a 
set  of  3DOF  miniature  rotorcrafts.  This  demonstrates  the  performance  and  feasibility  of 
the  proposed  method.  The  performance  of  the  method  is  also  demonstrated  by  simulating 
it  in  Visual  C++  by  using  RHC  Object  Oriented  Library  (RHCOOL),  which  is  adapted 
for  Dynamic  Scheduling  of  Decentralized  RllC  Systems. 
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Introduction 

Scheduling  of  RHC  computational  and  communication  is  very  important  for 
decentralized  RHC  control  since  it  allows  optimization  of  resources  to  achieve  maximum 
performance  in  the  presence  of  uncertainty,  disturbances,  and  delays.  The  tasks  addressed 
in  this  report  related  to  the  project  entitled  “Real-time  Scheduling  of  Multiple  Uncertain 
Receding  Horizon  Control  Systems”  are  as  follows: 

Task  12:  Validate  RHC  performance  via  Matlab  simulations,  and  distributed  hard 
real-time  simulations  running  on  a  parallel  cluster  of  digital  processors. 

Task  13:  Test  RHC  and  scheduling  algorithms  on  multi-rotorcraft  experimental  test¬ 
bed. 

Task  14:  Documentation  and  explanation  of  software  and  results 

For  these  tasks,  we  designed  a  scalable,  dynamic  run-time  scheduling  algorithm  for 
multiple  RHC  running  on  distributed  digital  processors  (task  #11)  that  has  been  delivered 
in  the  previous  report.  In  this  work,  dynamic  scheduling  algorithms  for  both  computation 
and  communication  scheduling  of  multiple  processors  are  developed  that  directly 
optimize  robust  RHC  performance  of  the  whole  network.  This  is  achieved  through 
minimization  of  the  upper  bound  on  the  so-called  overall  closed-loop  cost  of  the  system, 
subject  to  scheduling  constraints  on  both  computation  and  communication. 

Feasibility  and  performance  of  the  proposed  scheduling  methods  was  demonstrated 
through  C++  real-time  simulations  running  on  a  parallel  cluster  of  digital  proeessors.  It 
was  done  by  having  different  sets  of  simulations  and  scenarios: 

a)  The  performance  of  distributed  RHC  was  demonstrated  by  using  multiple 
miniature  helicopters  in  the  formation  with  trajectory  generation  and  obstacle 
avoidanee.  Dynamic  formation  was  also  demonstrated  brielly  (task  #12). 

b)  The  proposed  scheduling  methods  were  examined  with  two  sets  of  simulation; 
first,  by  using  two  computers  each  one  having  two  hovercrafts  while  maintaining 
their  formation;  second  by  using  four  hovercrafts  with  four  processors  (task  #12). 

e)  Distributed  RHC  simulations  with  varying  eommunications  and  . . .  (Hojjat) 

In  addition,  the  performanee  of  the  RHC  method  was  demonstrated  by  applying  it  on  the 
miniature  helieopters.  Furthermore,  the  foresaid  scheduling  methods  are  applied  on  some 
sets  of  3DOF  miniature  under  actuated  rotorcrafts.  These  rotorcrafts  are  the  modified 
version  of  the  miniature  underaetuated  hovercrafts.  It  was  done  by  using  four  hovercrafts 
with  four  processors.  In  both  tests,  overhead  vision  system  is  used  to  obtain  neeessary 
information  for  feedback  (task  #13). 

Finally,  all  of  the  programs  and  main  functions  used  to  obtain  the  foresaid  results,  were 
gathered  together  for  easy  aecessing.  All  of  the  programs  and  results  are  stored  on  the 
complementary  CDs,  and  they  are  explained  in  the  Appendix  (Task  #14). 

The  following  chapters  are  included  in  this  report: 
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In  chapter  two,  a  scalable,  dynamic  nin-timc  scheduling  algorithm  for  multiple  RHC 
running  on  distributed  digital  processors  that  has  been  developed  recently  and  explained 
in  [2]  is  explained  briefly.  In  this  chapter,  a  new  dynamic  scheduling  approach  is 
proposed  that  considers  the  effect  of  modeling  uncertainty  for  multiple  continuous  time 
RHC  systems.  This  is  accomplished  by  combining  a  scheduling  approach  with  results 
from  continuous  time  nonlinear  systems  theory.  Assuming  the  prediction  horizon  and 
communication  period  are  two  variables,  using  a  limited  computing,  and  communicating 
resource,  a  new  technique  is  proposed  for  determining  the  execution  horizon  and 
communication  period  for  multiple  distributed  RHC  systems.  The  problem  of 
determining  the  execution  horizon  for  each  subsystem  while  optimizing  the  performance 
is  cast  into  a  constrained  optimization  problem. 

In  chapter  three,  application  to  3DOF  under-actuated  miniature  hovercraft  problem  has 
been  investigated.  Since  RHC  approach  is  a  model-based  algorithm,  modeling  and 
identification  of  the  apparatus  has  been  done,  briefly.  In  addition,  using  the  RHCOOL, 
the  dynamic  scheduling  of  multiple  hovercrafts  has  been  investigated  based  on  the 
designed  scheduling  method.  In  this  investigation,  sets  of  two  and  four  processors  are 
being  used  to  control  four  hovercrafts  with  RHC  method,  while  the  computation  and 
communication  is  being  scheduled  using  the  proposed  dynamic  scheduling  approach. 

In  chapter  four,  experimental  results  have  been  reported  for  miniature  helicopter  and 
hovercrafts.  The  results  are  from  implementing  RHC  approach  to  single  helicopter  as 
well  as  scheduling  results  of  multiple  processors  (two  and  four)  with  four  hovercrafts. 

Conclusions  and  future  work  are  presented  in  chapter  five  and  references  are  in  chapter 
six. 

Chapter  2  of  this  report  is  describing  [2]  in  brief,  chapter  3  is  describing  Task  #12,  and 
chapter  4  of  the  report  is  describing  Task  #13  of  the  first  proposal.  In  addition.  Appendix 
A  is  explaining  the  materials  on  the  complementary  CDs  (Task  #14). 
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8.1  Dynamic  Scheduling  Algorithm  for  Multiple  RHC  Running  on 
Distributed  Digital  Processors 

This  chapter  briefly  describes  the  dynamic  scheduling  approach  recently  developed  at  the 
CIS  lab.  Part  of  the  result  was  recently  published  on  [1],  while  the  whole  chapter 
described  in  [2], 


8.1.1  Scheduling  of  Multiple  RHC  Systems  without  Coupling  on  a  Single  Processor 

Consider  the  problem  of  controlling  n  decoupled  uncertain  nonlinear  systems  using  the 
RllC  scheme.  The  following  equations  serve  as  the  nominal  model  for  describing  the 
decoupled  plants 


(1) 


in  addition,  the  actual  plants  arc  described  by 

X,-  =  f,  (x/  (0,  u ,•  (OT  )  +  g ,  (X , ,  u , ,  0,  /'  =  1 , . . . , « 


(2) 


For  the  proposed  approach,  the  execution  horizons  of  all  subsystems  should  be  defined 
such  that  the  overall  performance  of  the  system  is  maximized.  In  order  to  evaluate  the 
performance  of  the  system,  the  following  cost  function  is  proposed  as  the  cost  of  the 
closed  loop  system  from  time  t  to  /  +  T”,,  where  T,  is  the  period  that  calculated  execution 
horizons  would  be  applied  to 


n  {  iJ, 


i=l  V/t=l 


.1  j-  i  V 


(3) 


where  c/,.  =[7; =t  +  (/c-l)d],  and  u’ ,(r;x,(/^ ))  is  the  optimal  input  applied  to 
subsystem  /. 

rt 

The  idea  is  to  find  the  execution  horizon  of  each  subsystem  (<?,  ),  such  that  is 

i=i 

minimum.  As  explained  in  [2],  instead  of  calculating  ,  the  following  cost  is  proposed 
which  is  an  estimation  of  the  cost  in  (7): 


Floor  function  of  a  real  number  x,  denoted  [.vj ,  is  the  largest  integer  less  than  or  equal  to  .v. 
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(4) 


Proposition! ; 

Suppose  the  following  assumptions  hold  true: 

5.  ry,  is  quadratie,  so:  ^,(x,,u  J=  -h 

6-  0,  (x,  )  =  xf2,x,.  is  Lipschitz  continuous  with  constant  /^ . 

7.  f,  in  equation  (5)  is  Lipschitz  continuous  with  constant  L, 

8.  g;  in  equation  (6)  is  bounded  and  ||g,(x,,u.)||  . 

then  the  following  inequality  exists: 


P 


\\(5) 
JJ 


where  is  the  bound  on  measurement  errors  caused  by  sensor  noise. 


Proof:  See  12]. 


Proposition  2: 

Consider  n  decoupled  systems  with  equations  presented  in  (5)  and  (6),  controlled  by  RHC 
using  only  one  computer.  In  addition,  the  assumptions  of  proposition  1  arc  valid.  Since 
uncertainties  in  the  subsystems  arc  different  and  the  measurements  are  performed  with 
bounded  sensor  noise,  show  that  the  following  equation  can  be  used  to  optimally 
determine  the  execution  horizons  of  all  subsystems: 


minV 

s.  ■“ 


'r 

( 

/  ,  w 

)+6i  — -l)-<5, 

i-. 

y 

^  'U.  ^  J, 

s.t. 


k^<n(2--l) 
.  4.,' <  <5,  S 


(6) 
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where  d^f,,  and  are  the  maximum  and  minimum  acceptable  execution  horizons  for 
subsystem  /,  respectively.  is  the  execution  horiz.on  of  subsystem  /  before  being 
updated. 

Proof:  See  12] 


8.1.2  Scheduling  of  Multiple  Systems  with  Coupling  Effect  on  a  Single  Processor 

Definition  1.  The  set  is  called  the  neighboring  set  of  subsystem  /,  and  consists  of  any 
subsystem  in  the  network  that  has  direct  interconnection  with  the  subsystem  /. 


Assumption  1.  Let  us  consider  the  following  inequality: 

||x (f  +  J )  -  X ,  (/  +  d',  f  I  <  fi,.  (r!) ,  X ,  (/ ),  X ,  (/ ))  ( 7 ) 

This  means  that  by  knowing  the  actual  and  nominal  states  at  time  t  (i.e.,  x,(f)  and  x,(/)), 
the  error  in  estimating  the  norm  of  state  variables  of  subsystem  /  is  less  than  at  time 
t^S. 


Remark  1.  As  explained  in  [2],  in  equation  (25)  is  a  function  of  S  with  the  state 

dependent  parameters.  These  parameters  arc  updated  at  the  beginning  of  the  scheduling 
algorithm  and  do  not  change  during  the  operation  of  scheduling  algorithm  for  finding 
new  execution  horizons  (periods).  Therefore,  without  loss  of  generality  we  consider 

as  a  function  of  5 ,  i.e.  BfS)  in  the  remaining  parts  of  this  section. 


Problem  Formulation 

Consider  a  network  of  n  dynamically  decoupled  subsystems  using  the  RIIC  approach  in  a 
decentralized  manner.  The  cost  function  associated  to  each  subsystem  can  be  expressed 
as  follows: 


J,.(^7',x,,u,.,x,.)=  ^,(x,.(r,f),u,(r))+ 


(«) 


where  ^,(x,(r,/),Uj(r))=  xf^(r,f)0,x,(r,f)-t- u,- (r)/?^u,(z')  and  x,  is  a  vector  containing  the 
states  of  all  neighboring  subsystems  of  the  subsystem,  is  a  function  which  defines 
the  interaction  between  two  nodes  of  the  system  (network)  x,  and  x^  . 
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Remark  1.  For  simplicity  of  the  proof,  it  supposed  that  the  interaction  function  is  just 
between  two  nodes  of  the  system,  i.e.  =g^(x.,x^).  After  that,  the  formulation  is 
generalized  for  any  combinations. 


As  an  extension  to  the  (9),  the  following  equation  is  proposed  as  an  estimation  of  the 
elosed  loop  cost  in  the  existence  of  coupling  between  subsystems. 


/=i  I  I  ye.-r  i 


(9) 


Lemma  1.  Consider  the  following  assumptions: 

4-  If  X,  and  x^  are  two  mx\  column  vectors,  x^  is  a  2/77x1  column  vector  such 

that  \y  =  [x/,x/[  . 

5-  Let  be  Lipschitz  continuous  with  constant  ,  and  g*.  (x^)  be  a  positive 

scalar. 

6-  From  Assumption  1,  we  can  have:  ||x,(r  +  d')- x,(f  +  (3’|  <  and 

||x^(f +  d')-x,.(f +  d'|< 

Show  that: 

|g^.  (x,.  (/  +  S),  x/f  +  J))-  gj,  (x,  (/  +  c!) ),  X ,  {f  +  <;>  )|  <  (Z?,  +  Bj)  (10) 


Proof:  See  [2]. 


Corollary  1.  Combination  of  Proposition  1  and  Lemma  1,  results  in  the  following 
inequality': 


f"'"'  ^,(x,(z-),u;,(r;x, (/)))+  ^g^xXO-x/r))  c/r< 

V  / 

+  [  Z  (xr,7  (^;  x,  (0)^  A.j  (^;  X  /  (0))+  -t)+B\T- 1 ))) 


(11) 


j€.Ai 
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The  following  optimization  problem  is  proposed  for  scheduling  of  n  dynamically 
decoupled  subsystems  in  an  interconnected  network,  on  a  single  processor: 


min 

<5, 


(=1 


Y [  V'  r,  X ,  (?  -  , ))  u ; ,  (r;  x ,  (f  -  ,  )))+P,B,{r-l  )\It 


+  ■ 


r  I-®* 


(x  *7-,,  (z-;  X ,  fr  -  ft  X  rj  (r;  \j(t-  ))) 


<Jt 


(12) 


S.t.  i 


-1) 

^lh.i  <  -  ^uhj 


If  B-G  lemma  is  used  for  finding  ,  and  noise  in  measurement  is  considered  (like 
Proposition  1  ),  we  would  have: 


B.  =Z7  -1-^ 

'  L 


Therefore,  the  scheduling  optimization  problem  can  be  expressed  as: 


(13) 


f  rl  +  Sj  (  *  1  i 

\\  *  i 

'  i 

J,  9/lxr,,lGx,( 

)\»tJ 

.r;x,( 

/-^p./lft 

min 

S 


7=1 


a, 


S; 


Li 


kL. 


S: 


Za 

I  jsAi 


+  - 


UPi 


+ 


L, 


1  • 

/ 

*  1 

Vr.i 

/ 

(r;x, 

x/- ^  (r;x  ^ 

-1  +/7, 

S.t  \ 

\ 

f 

/  ' 

V 

\ 

c' 

(  .  7 

/  ‘ 

J) 

\ 

— ^ 

YiO. 

L,  ' 

W 


J) 


JJ 


S.t.  ( 


^Ib.i  <  -  ^uh,i 


(14) 


Remark  1.  The  formulation,  which  is  presented  here,  is  compatible  for  decentralized 
application,  since  each  node  has  a  special  cost  function.  Extension  of  the  present 
formulation  will  be  done  for  the  cases  of  having  more  than  one  processor,  sharing 
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the  information  via  a  communication  network. 


8.1.3  Computation  and  Communication  Scheduling  of  Multiple  Systems  on 
Multiple  Distributed  Processors 

The  problem  under  study  is  illustrated  in  Figure  4.  For  this  case,  we  consider  delay  in 
communication  and  we  determine  execution  horizon  and  communication  period  for  each 
subsystem. 


Processor  1  Processor  2  Processor  3 


t'ompiitalion 


Communications 

Figure  1-  Illustration  of  Computation  and  Communication  Scheduling  for  the  case  of  three 
processors  and  presented  configuration. 

Suppose  having  two  computers  connected  via  a  local  network  and  these  computers  have 
/7|  and  772  number  of  subsystems  using  RHC  approach,  respectively.  The  total  number  of 
systems  are  n^, .  Therefore,  we  have;  + 112  .  The  method  is  later  generalized  for 

more  than  two  computers. 

Remark  1:  Communication  period  should  be  where  1^,  is  the  communication 

period  of  system  /,  Sj  is  the  execution  horizon,  and  z,  is  an  integer. 

Remark  2:  Maximum  communication  delay  of  the  data  sent  from  node  i  to  node  j,  is 
equal  to: 
r//  =  Ci  + 

where  is  the  delay  because  of  senders  and  receivers  and  this  can  be  considered  as  a 
fixed  (known)  delay. 
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When  the  data  sent  from  subsystem  7,  has  delay  equal  to  r,y ,  the  open-loop  cost  of 
subsystem  i,  can  be  expressed  as  follows: 


g,(x;(r;/),u,(r))+  ^g*(x,(r;/),x^(r;f-r,y))  c/r  (15) 


jeA, 


Like  before  (equation  (9)),  we  propose  the  following  cost  as  an  estimation  of  the  closed 
loop  cost: 


\ 
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Based  on  the  worst  case  analysis,  for  the  open  loop  cost  function,  we  have  (note  that  we 
are  using  the  produced  trajectory  of  other  subsystems  instead  of  estimating  the  trajectory 
of  other  subsystems): 


\ 

:*(r;0.U7y,(r 

;(r;/),x*(r;/-ry)) 

c/r 

V 

/ 

(17) 


where  we  have  two  cases  for  r,y : 

{r,y  =  S I  ,  if  i  and  j  are  in  the  same  computer 
Tjj  =  ZjSj  +  <^jj  ,  if  i  and  j  are  in  different  computers 

Remark:  It  should  be  noted  that  this  is  valid  for  the  case  that  we  arc  using  the  produced 
trajectory  by  the  neighbors. 

The  following  optimization  problem  is  proposed  for  computation  and  communication 
scheduling  of  n^  subsystems  on  two  computers,  by  having  the  assumption  that  4”,  =  : 

(computation  scheduling  means  finding  the  proper  values  for  <5,  and  the  communication 
scheduling  means  finding  the  proper  communication  rate  (or  period)  for  each  subsystem. 
The  combination  of  these  two  problems  leads  to  finding  5^  and  .) 
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where  B,  is  the  amount  of  Bytes  to  be  sent  in  eaeh  eommunication  of  system  /,  and  BW 
is  the  Bandwidth  of  the  network.  C,  is  defined  from  the  following  (same  as  the  previous 
seheduling  cost): 


If  B-G  lemma  is  used  for  finding  ,  and  noise  in  measurement  is  considered  (like 
Proposition  1),  we  would  have; 

-t)  (20) 


Combination  of  (46)  and  (20)  by  assuming  no  noise  in  measurement  (i.c.,  =0), 

results  in: 
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Generalization  of  the  approach  to  the  cases  of  having  more  than  two  processors  is  done  in 
the  following. 

Extension  of  the  presented  approach  is  almost  straightforward  except  the  constraint  on 
the  communication  bandwidth.  The  following  definition  helps  us  in  this  matter. 

Definition  2:  is  the  set  of  processors  that  have  at  least  one  neighbor  of  subsystem  /. 

Note  that  the  proeessor  having  subsystem  /  on  it,  is  not  considered  in  C, . 

Using  the  definition  of  C, ,  the  constraint  on  communication  bandwidth  can  be  defined  as 
follows: 


/=!  jeC, 


8.2  Distributed  Real-time  Simulations  with  Application  to  3DOF 

Miniature  Helicopter  and  Hovercraft  Problem 

In  the  following  two  chapters,  first  the  distributed  RHC  performance  is  studied  by 
applying  the  method  to  multiple  miniature  helicopter  systems.  Secondly,  the  proposed 
dynamic  scheduling  approach  is  applied  to  multiple  hovercraft  systems  on  distributed 
computers. 

8.2.1  Distributed  RHC  of  Multi-Vehicle  Systems 

This  chapter  briefly  describes  the  distributed  Receding  Horizon  controllers  of  multi 
helicopter  systems  recently  developed  at  the  CIS  lab  [4]. 

In  this  section,  distributed  RHC  with  three  vehicles  are  investigated.  The  problem  is 
formation  of  three  vehicles  for  some  leader-follower  examples.  The  examples  are  consists 
of  real-time  simulations  with  obstacle  avoidance  and  dynamic  formation. 

A  battlefield  scenario  is  set  up  for  the  distributed  receding  horizon  control  of  three 
helicopters  chasing  one  target,  avoiding  one  obstacle  while  keeping  a  triangular 
formation.  Different  cost  functions  arc  applied  for  each  helicopter.  Preliminary 
simulation  results  of  dynamic  formation  control,  dynamically  adding  and  reducing 
helicopters  and  dynamically  adding  and  reducing  targets  arc  also  presented  in  this 
section. 

8.2. 1.1  Problem  Setup  and  Proposed  Method  of  Solution 

RHCOOL  [3],  Receding  Horizon  Control  Object-Oriented  Library,  is  used  for  this 
simulation.  This  library  aims  at  precisely  and  conveniently  simulating  RHC  control  of  a 
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model  or  models  by  declaring  different  parameters  and  cost  functions  for  each  model. 
Since  in  this  application,  the  helicopters  arc  assumed  to  be  same  in  dynamics  but 
different  in  their  individual  tasks,  different  cost  functions  arc  developed  for  each  vehicle 
in  the  target  following,  obstacle  avoidance  and  formation  maintenance. 

Three  computers  and  a  gigabit  fast  Ethernet  switch  arc  used  for  the  distributed 
simulation.  In  addition,  UDP  protocol  is  applied  for  data  transmission  in  order  to 
guarantee  the  high-speed  data  exchange  between  those  systems.  However,  the  whole 
process  is  not  in  real  time  manner,  but  in  a  sequential  manner  to  prevent  any  possible  data 
lost.  In  this  section,  the  simulations  arc  nin  on  both  one  computer  and  on  three  computers 
in  a  distributed  manner,  and  the  results  are  compared. 

In  [42],  the  concept  of  agent  brigade  is  applied  to  robotic  soccer  team.  This  aspire 
us  using  a  subsystem  concept  in  the  dynamic  formation  control  simulation.  A  group  of 
helicopters  chasing  one  target  is  considered  as  one  individual  system.  Each  helicopter 
within  this  system  is  divided  into  subsystem  performing  its  individual  task.  The 
simulation  is  run  on  one  computer,  on  which  each  helicopter  control  process  is  operated 
sequentially  in  a  non  real  time  manner.  Two  targets  arc  put  into  battlefield  one  after 
anther. 

The  task  is  divided  into  two  major  parts,  a  path  planner,  which  chases  the  target  and 
avoids  the  obstacle  on  the  way,  and  two  followers  that  catch  up  with  the  planner  and  keep 
a  certain  distance  from  each  other. 

In  [40],  a  choice  of  cost  function  for  the  formation  of  multiple  UAVs  is  introduced. 
Moreover,  for  the  cost  function  of  path  planner  in  this  simulation,  which  is  responsible 
for  tracking  the  target  and  doing  the  killing,  is  shown  below. 

J,  (k  )  =  Jl‘  (\,(k))  +  J^(^^  (l< ))  +  (f< ))  (23) 

where 

J:'(k)  =  ^(yJk)-x,(k)/Q(yJk)-x,(k))  (24) 


J''  (  xjk ))  is  used  for  target  tracking  the  target,  y  j(k  )  is  the  position  data  of  the 
target  at  time  k  ,  and  x^(k)  is  the  position  data  of  the  path  planner  at  time  k 

In  this  chasing  scenario,  an  obstacle  will  be  on  the  way  of  the  three  helicopters 
formation  and  they  need  to  avoid  colliding  with  it.  In  [41],  a  potential  function  is 
introduced  into  the  cost  function  of  RHC  controller  for  the  obstacle  avoidance  task. 
j^oc(k  j  function  is  used  for  the  obstacle  avoidance. 


J^"‘‘(k)  = 


_ K _ 

( xjfxjkj-x^y  +(}\(k)-y„)'  - RR)' 


(25) 
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where  ( x^(k ),y\(k))  designate  the  position  data  of  the  path  planner  at  time  k, 
while  (x^,y^)  is  the  coordinate  data  of  the  obstacle,  and  RR  is  the  distance  between  the 
center  of  the  obstacle  and  the  path  planner. 

Even  though  the  combination  of  Jl'(\^(k))  and  J”‘“'(k)  is  enough  for  trajectory 
following,  only  this  two  part  will  not  give  us  satisfying  result  of  simulation.  Because  no 
matter  how  large  the  cost  will  be,  RllC  controller  will  give  the  leader  a  huge  input  to 
decrease  the  cost  to  zero,  an  input  that  is  too  huge  to  be  implemented  in  reality  and  that  is 
able  to  make  the  helicopter  “jump”  to  the  target  at  time  k  +  I  from  where  it  is  at  time  k  . 
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Figure  2-  without  velocity  penalty 

Therefore,  another  function  k )  to  slow  down  the  leader  is  needed  in  the  cost 
function. 

j;“'"(k)=j3,(i<,(ky-+v,(ky)  (26) 

jy''(k)  is  a  penalty  is  the  cost  function  for  the  path  planner,  where  ii^(k)  and 
v^(k)  are  the  axial  velocity  and  radial  velocity  of  the  path  planner  respectively.  After 
adding  this  part  to  the  cost  function,  the  path  of  the  leader  becomes  satisfying. 
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path 


Figure  3-  with  velocity  penalty 

For  the  followers  in  this  system,  there  is  another  set  of  cost  functions  for  each  of 

them. 


j,=jru^(k))+jr(x(k))-^jr(Mk)) 


ri7o>v 


(27) 


(28) 


J,  is  responsible  for  /-th  follower  to  keep  the  formation,  (x^,y.)  is  the  position 
data  of  the  /-th  vehicle,  while  ( x^,y  ■ )  is  the  data  position  of  other  /  vehicles.  R  is  used  to 
keep  the  distance  between  each  two  helicopters. 

J:''^'^=l3,(u,(kf+v,(k)-)  (29) 

Same  as  the  path  planner,  each  /-th  helicopter  needs  a  penalty  function  to  slow 
down  the  velocity  of  them. 


For  each  /-th  vehicle,  there  is  another  function  to  protect  each  helicopter  would  not 
crash  into  each  other.  Even  though  the  J/”'  function  is  responsible  and  doing  a  fairly 

good  job  on  keeping  formation,  there  is  still  high  possibility  for  some  of  the  vehicles 
crashing  into  each  other  when  the  path  planner  is  moving  too  fast. 
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Rj 


Figure  4-  helicopter  fomiation 


As  in  Figure  4,  ideally,  /?,  and  /?,  should  be  equal  with  the  length  of  the  triangle 
formation/?.  However,  the  leader  is  set  to  follow  the  target  and  avoid  obstaele  only, 
whieh  means  formation  is  not  in  its  consideration.  Therefore,  when  leader  is  far  away 
from  the  target,  it  tends  to  move  faster  since  its  cost  function  is  large;  on  the  other  hand, 
the  followers  are  not  able  to  accelerate  until  their  distance  from  the  leader  /?,  becomes 
larger.  This  delay  will  cause  J/'”'(k)  for  the  followers  increase  significantly  and 

dominate  the  whole  cost  function,  while  J/'”'  (k )  has  weaker  influence,  so  the  distance 

/  ^  1 ,  /  / 

between  the  followers  could  not  be  maintained.  The  followers  would  only  collide 
with  each  other,  since  /?,  is  impossible  to  increase  because  of  the  optimization  of  the  cost 
function. 

Therefore  another  function  J""'  should  be  introduced  into  the  cost  function  for  the 
followers. 


[ (l<)-.x/k)f  +  (.V, (k)-y/k >)“  - j 

where  is  the  safe  radius  for  the  followers.  is  a  relatively  small  number,  small 

enough  for  not  interfering  the  formation  function,  but  larger  than  the  diameter  of 
helicopter  so  that  they  would  not  collide.  In  this  simulation,  is  set  to  5m.  Fven 

though  in  Figure  3  there  is  a  crossing  between  these  two  helicopters,  they  actually  avoid 
that  collision,  as  shown  in  Figure  5,  which  reflects  the  distance  between  the  two 
followers. 
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Figure  5-  Distance  between  two  followers 
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8.2. 1.2  Distributed  Target  Following  and  Obstacle  Avoidance  Simulation 

In  Figure  6,  the  structure  of  distributed  simulation  is  illustrated,  and  the  procedure 
is  followed  after  that. 


Figure  6-  Three  computers  distributed  simulation 


1.  Calculate  inputs  and  next  position  of  leader  based  on  the  target  position  data 
generated 

2.  Send  leader’s  position  to  each  follower 

3.  Send  follower  ll’s  position 

4.  Calculate  inputs  and  next  position  of  follower  1  based  on  the  data  received  from 
leader  and  follower  11 

5.  Send  follower  I’s  position 

6.  Calculate  inputs  and  next  position  of  follower  11  based  on  the  data  received 
from  leader  and  follower  1 

7.  Send  positions  of  follower  1  and  11  to  the  leader 

8.  Go  back  to  1 
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The  simulation  is  in  sequential  manner,  which  means  one  task  will  take  place  only  if  the 
task  ahead  of  it  is  finished  to  make  sure  no  or  least  data  lost  due  to  timing  of  data  transfer 
in  the  network.  However,  in  the  section  (3.2)  the  simulations  arc  done  using  the  proposed 
real-time  scheduler  for  data  communication. 

In  Figure  7,  the  scenario  is  that  the  target  vehicle  makes  a  turn,  while  the  group  of  the 
three  helicopters  avoids  the  round  obstacle  when  chasing  it.  In  Figure  8,  scenario  is  more 
complicated.  The  target  is  moving  in  a  circle  path,  and  the  group  of  the  helicopters  avoids 
the  round  obstacle  twice  on  its  way  of  chasing  the  target. 


path 


X 


Figure  7-  distributed  simulation  01 


path 
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8.2. 1.3  Dynamic  Formation 

Subsystem  concept  is  used  in  the  dynamic  formation  control  simulation.  A  group  of 
helicopters  chasing  one  target  is  considered  as  one  individual  system.  Each  helicopter 
within  this  system  is  divided  into  subsystem  performing  its  individual  task. 

In  this  section,  a  group  of  six  helicopters,  first,  forms  a  triangular  formation  and  chases  a 
target,  which  is  moving  far  away.  Then  at  time  another  target  shows  up  in  the  scenario. 

The  six  helicopters  will  divide  into  two  separate  groups,  three  vehicles  for  each  group, 
and  chase  the  two  targets  separately. 

Each  group  contains  one  path  planner  as  the  leader,  and  two  followers  to  form  a 
smaller  triangular  team.  The  cost  functions  for  each  leader  and  follower  could  be  in  the 
same  way  of  the  cost  functions  as  discussed  in  part  2  of  this  paper,  although  some  values 
of  the  parameters  might  need  to  be  changed  for  better  performance. 


path 
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Figure  9-  dynamic  formation  simulation 


However,  this  dynamic  formation  simulation  in  Figure  9  is  only  a  preliminary  test 
for  the  feasibility  of  the  concept  of  the  subsystem  concept.  Therefore,  there  is  no  obstacle 
avoidance  simulation  at  this  point. 


8.2.2  Real-Time  Scheduling  of  Distributed  RHC 

All  of  the  simulation  presented  in  this  chapter,  considers  the  hovercraft  model.  Therefore, 
the  miniature  hovercraft  model  under  study  is  presented. 

8.2.2. 1  Hovercraft  Model 

A  complete  discussion  on  the  modeling  of  the  RC  hovercraft  is  presented  in  [37].  Since 
only  viscous  friction  is  considered  in  the  nominal  model,  the  model  is  similar  to  that 
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presented  in  [22],  From  the  results  of  [37],  it  is  straightforward  to  assume  a  linear 
relationship  between  the  applied  voltage  and  the  thrust  produeed  by  the  propellers. 
Therefore,  based  on  Figure  5,  the  nominal  model  expressed  in  local  coordinate  is  given 
as: 

it  =  ^{a^V^+a,V,-b^  u)^vr 
M 

v  =  --^b2V-ur  (31) 

M 

where  M  is  the  mass,  J  is  the  moment  of  inertia  about  the  Z-axis,  and  b^,  and  /j,  arc 
the  coefficients  of  viscous  friction  in  X,  Y,  and  rotational  directions,  respectively.  Also, 
F  and  V,  arc  the  applied  voltages  to  the  right  and  left  propeller  DC  motors,  respectively. 


Another  model  of  the  RC  hovercraft  with  more  details  is  considered  as  the  actual  model 
of  the  system  and  used  in  simulations.  This  model  is  as  follows  [37]: 


where  is  the  moment  of  inertia  of  propeller  and  rotating  part  of  the  DC  motor, 
and  cOp  ,  arc  the  angular  speed  of  the  right  and  left  propellers,  respectively,  d  and  (/>  arc 
the  pitch  and  roll  angels  of  hovercraft  indicating  the  ground  slop.  „ ,  and  /, ,  arc 

the  friction  functions  presented  in  [37]. 

This  model  includes  actuator  dynamics,  propeller  dynamics,  effect  of  hovercraft  speed  on 
the  thrust  produced  by  propeller,  nonlincaritics  in  friction  applied  in  hovercraft,  and  the 
ground  inclination.  In  addition,  noises  in  measurements  arc  applied  in  simulations. 
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Figure  10-  RC  hovercraft  model  with  produced  thrusts  by  propellers  and  local  (body)  and 
global  coordinate  systems 

8. 2. 2. 2  Scheduling  Real-Time  Simulation 

Real-time  simulations  for  scheduling  of  multiple  hovcrcrafts  on  multiple  processors  arc 
presented.  These  simulations  are  based  on  the  proposed  scheduling  method  presented  in 
[2]  and  explained  brietly  in  chapter  2.  The  problem  is  to  determine  the  execution  horizon 
and  communication  period  of  every  vehicle,  dynamically  based  on  the  overall 
performance  of  the  system.  The  problem  is  illustrated  for  the  case  of  three  processors  in 
Figure  4. 

In  this  section,  the  vehicles  are  considered  in  the  leader-follower  case.  The  leader 
produces  the  trajectory  while  the  followers  maintain  a  formation  with  respect  to  each 
others  and  the  leader.  Two  cases  arc  studied: 

Case  I  :  two  processors,  each  one  having  two  vehicles 
Case2'.  four  processors,  each  one  having  a  single  vehicle 

8.2. 2.2.1  Scheduling  Real-Time  Simulation  (Case  I) 

Formation  of  four  vehicles  is  considered  in  this  example.  Two  computers  (processors)  arc 
used  and  each  one  has  two  hovcrcrafts  (Figure  1 1).  Different  communication  delays  and 
communication  bandwidths  are  studied  in  this  example. 

Figure  1 1-  Schematic  diagram  of  case  1;  two  computers  and  four  vehicles 

The  nominal  model  of  hovcrcrafts  is  presented  in  (31).  All  of  them  have  similar  nominal 
model,  expressed  by  the  following  parameters: 

(Parameters  of  the  nominal  model  for  all  hovcrcrafts) 

The  actual  model  of  these  hovcrcrafts  is  expressed  by  (32).  (add  g  to  equations) 
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It  is  supposed  that  hovercrafts  1,  2,  and  4  have  same  levels  of  uncertainties  and  equal  to 
b=0. 1  (See  Proposition  1  for  definition  of  b),  and  hovercraft  3  has  higher  uncertainty 
b=3.0.  Note  that  all  units  in  this  example  are  in  SI  Units. 

The  controller  parameters  arc  similar  for  all  of  the  systems  and  arc  as  follows: 

(controller  parameters  in  a  table) 

The  cost  function  associated  to  each  hovercraft  can  be  expressed  as: 

Hovercraft  1  (leader) 

(cost  function) 

Hovercrafts  2,  3,  4  (followers) 

(cost  function) 

The  communication  delay  between  two  computers  is  around  0.2  s.  In  the  following 
figures,  the  results  for  the  case  of  trajectory  following  is  presented. 


Figure  12-  Trajectory,  followed  by  the  hovercrafts 
Figure  13-  Snapshots  in  different  times  to  show  the  formation 
Figure  14-  Computation  scheduling  results  for  both  CPUs  (execution  horizon  vs.  time) 
Figure  15-  Communication  scheduling  results  (communication  period  vs.  time) 


In  order  to  emphasize  the  effectiveness  of  the  proposed  dynamic  scheduling  algorithm, 
the  similar  situation  is  simulated  with  a  static  scheduling  algorithm  and  the  result  of 
trajectory  following  is  presented  in  the  following  figures. 

Figure  16-  Trajectory,  followed  by  the  hovercrafts 

Figure  1 7-  Snapshots  in  different  times  to  show  the  formation 

The  differences  in  comparing  figures  (13)  and  (14)  to  figures  (17)  and  (18),  is  only 
because  of  the  scheduling  method. 

8.2. 2. 2. 2  Scheduling  Real-Time  Simulation  (Case  II) 

Formation  of  four  vehicles  is  studied  on  the  cluster  of  four  computers.  In  this  example, 
distributed  RHC  and  Communication  scheduling  is  focused.  Since  obstacle  avoidance 
was  simulated  in  section  (8.2.1),  it  is  not  considered  in  this  example,  and  the  case  of 
dynamic  formation  is  added. 

The  hovercrafts  used  in  this  example  were  similar  to  the  previous  case.  The  distances  of 
vehicles  arc  changing  while  following  their  trajectory.  The  cost  functions  and  controller 
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parameters  were  similar  to  the  previous  case.  The  results  are  presented  in  the  following 
figures. 

Figure  18-  Trajectories,  followed  by  the  hovcrcrafts 
Figure  19-  Snapshots  in  different  times  to  show  the  actual  fonnation  and  the  desired  formation 
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8.3  Experimental  Verification 

In  this  chapter,  the  result  of  applying  RUC  to  miniature  helicopter  and  hovercraft  is 
investigated.  Dynamic  scheduling  algorithms  explained  in  Chapter  2  for  multiple  systems 
on  distributed  computers  were  verified  using  real-time  simulations  in  chapter  3.  In  this 
chapter,  these  methods  are  applied  experimentally  to  multiple  hovercrafts  to  verify  the 
applicability  of  the  method.  In  addition,  the  RHC  method  was  applied  for  trajectory 
following  of  a  miniature  helicopter,  and  is  presented  here. 

In  the  following  chapters,  first,  the  apparatus  is  described  and  every  component  is 
explained;  after  that,  the  results  are  presented  for  different  cases. 


8.3.1  Description  of  the  Apparatus 

Figure  21  shows  a  schematic  diagram  of  the  setup.  The  setup  consists  of  the  following 
parts: 

4-  A  set  of  cameras  (Cl  to  C9)  used  to  capture  images  and  to  forward  to  the  Vision 
computers  (VI  to  V9).  Each  image  consists  of  some  colored  objects  and  the 
computers  calculate  the  centroid  of  each  object  (Figure  21).  Each  camera  is 
connected  to  one  computer  in  order  to  decrease  the  image  processing  time. 

5-  All  of  the  centeroids  calculated  by  the  vision  computers  are  transferred  to  the 
serv'er  via  a  Gigabyte  Ethernet  Switch.  The  server  combines  the  centroid 
information  and  sends  them  to  the  main  controller  computers  (Ml  to  M4)  via 
another  Gigabyte  Ethernet  Switch. 

6-  Ml  and  M2  receive  the  data  from  sensors.  One  sensor  is  the  vision  system  and  the 
data  is  sent  from  the  server,  and  the  other  sensor  is  a  wireless  orientation  sensor 
(Wireless  lnertiaCube3  [24])  and  calculate  the  input.  The  input  signals  are 
transferred  to  the  apparatus  via  data  acquisition  board  and  wireless  transmitter^^ 
(T1  and  T2).  In  this  setup  each  computer  is  connected  to  one  apparatuses. 


The  wireless  transmitter  is  “lliTec  Laser  6  Channel  VSMIlz  RC  Controller  Pkg”  and  is  explained  in  [28] 
and  [3 1  ]. 
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Figure  20-  Schematic  layout  of  experimental  setup 


Figure  21-  The  miniature  hovercraft  version  11 1 .0 
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Figure  23-  The  miniature  hovereraft  version  H2.0.  This  hovercraft  is  used  for  the  results  of  this 
report.  It  has  more  powerful  fans  and  its  model  is  more  similar  to  3DOF  miniature  helicopter. 


The  apparatus  used  in  this  investigation  is  the  miniature  hovercraft  shown  in  Figure  23. 
The  previous  versions  of  hovcrcrafts  arc  also  shown  in  figures  21  and  22.  This  hovercraft 
(Figure  23)  is  a  modified  version  of  the  R/C  hovercraft  [32].  The  thrusters  are  ducted  fans 
[39]  and  they  arc  more  powerful  than  the  previous  fans. 
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To  investigate  the  performance  of  RHC  approach  with  trajectory  tracking  of  helicopters, 
the  miniature  helicopter  as  shown  in  Figure  24,  is  used. 


Figure  24-  The  miniature  helicopter  that  used  for  the  results  of  this  report. 


For  controlling  the  apparatus,  a  wireless  transmitter  is  attached  to  the  computer  using  a 
data  acquisition  board  and  the  receiver  is  attached  to  the  apparatus.  In  order  to  connect 
the  transmitter  to  the  data  acquisition  board  some  modifications  arc  done  in  the 
transmitter.  The  data  acquisition  board,  transmitter  and  all  of  the  modifications  arc 
described  in  [28].  Two  batteries  arc  used  as  follows: 


3-  One  9.6  V  battery  used  for  the  main  fan  which  cause  flouting  of  hovercraft.  This 
fan  pushes  air  under  the  hovercraft  and  causes  it  to  move  on  a  film  of  air, 
therefore  the  friction  of  hovercraft  in  all  directions  (forward,  side,  and  angular)  is 
very  small. 

4-  One  7.4  V  battery  used  for  all  ducted  fans.  This  battery  is  connected  to  the 
receiver  and  all  fans  arc  connected  to  the  receiver. 


As  mentioned  above,  in  order  to  measure  orientation  and  angular  velocity  of  miniature 
hovercraft,  a  wireless  sensor  is  used.  This  sensor  is  a  Wireless  IncrtiaCubcS  from 
InterScnce  [24].  This  sensor  is  described  and  its  benefits  are  explained  in  [3].  The 
position  in  X-Y  coordinate  is  obtained  using  Vision  System  which  is  developed  in  the 
CIS  Lab  and  is  illustrated  in  the  following.  In  addition,  for  performing  these  experiments 
a  new  surface  without  slope  is  prepared  recently  in  CIS  Lab. 

8. 3. 1.1  Vision  Array 

In  order  to  have  the  position  (X-Y)  of  the  hovercraft  a  vision  array  is  built  at  a  height  of 
3.5  meters  in  the  test  area  of  the  CIS  Lab.  It  produces  a  test  area  in  the  shape  of  a  square 
with  dimensions  of  20  by  20  feet.  9  wcbcams^^  are  connected  to  9  computers  and  they 
send  the  data  to  a  main  computer  which  acts  as  a  server.  Sec  Figure  21,  and  Figure  36  to 


Installing  more  webcams  is  also  possible  but  with  four  webcams  the  working  area  is  covered  with  a 
reasonable  resolution. 
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Figure  38.  Figure  36  shows  an  overview  of  the  vision  array.  Figure  37  is  a  schematie 
drawing  showing  a  connection  point.  Figure  38  shows  a  connection  point  and  a  webcam. 

Each  computer  with  a  webcam  connected  to  it  can  get  the  picture  and  process  the  image 
within  less  than  0.05  of  a  second.  Therefore,  the  data  from  the  whole  vision  system  is 
updated  in  less  than  0.05  of  a  second. 


Figure  25-  An  overview  of  the  vision  array 
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Figure  26-  Schematic  drawing  of  a  connection  point 


Figure  27-  A  connection  point  with  a  webcam 


8.3.2  Formation  of  Four  Hovercrafts 


The  experimental  setup  is  shown  in  Figure  21.  As  illustrated  in  that  figure,  two  miniature 
hovercrafts  are  used  and  two  hovererafts  are  being  simulated. 

Nominal  models  of  all  vehicles  are  expressed  with  the  following  equations; 


(Equations  of  motion  for  hovercraft  112.0) 


where  ...  (explanation  of  parameters) 
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Using  a  similar  identification  algorithm  explained  in  [3],  the  parameters  of  both 
hovercrafts  are  identified  as  follows: 

(Parameters  of  hovercraft  1) 

(Parameters  of  hovercraft  2) 

For  hovercrafts  3  and  4,  the  parameters  arc  considered  to  be  similar  to  system  1 . 

The  RHC  controller  used  in  this  experiment  was  adapted  from  [6]  and  RllCOOL  [3]  is 
used  for  implementation. 

Parameters  of  RHC  controller  for  all  of  the  subsystems  arc  as  follows: 


Hovercraft  1 :  (change  the  table) 


Nx  =  6 

Number  of  states  ( u,  v,  r,  y/,  x^. ,  ) 

Nz  =  2 

Number  of  flat  outputs  {y/ ,  u) 

Nu  =  2 

Number  of  inputs  ( tr, ,  ih  ) 

Ny  =  2 

Number  of  outputs 

Nd  =  2 

Maximum  number  of  derivatives  from  flat  outputs 

Ns  =  4 

Number  of  Spline  control  points 

Ni  =  101 

Number  of  Spline  interpolation  points 

T  =  4.0  (sec) 

Prediction  Horizon 

Nc  =  Ns 

Number  of  varying  control  points 

1  method  =  1 

It  means  that  the  cubic  spline  is  used  for  interpolation 

Q 

R 

Hovercraft  2: 

(Add  the  table) 

Hovercrafts  3  and  4:  similar  to  hovercraft  1 

In  Figure  21  the  computers  used  for  controlling  the  hovercrafts  (Ml  to  M4)  are  all 
Pentium  4,  with  one  CPU  of  2.8  GHz  speed. 

This  example  is  a  leader  follower  with  a  fixed  formation.  Hovercraft  1  is  the  leader  and 
the  others  are  followers.  Dynamic  scheduler  defines  the  communication  rate  for  each 
vehicle.  In  the  following  figures,  trajectory,  inputs,  and  communication  period  of  all 
systems  are  presented. 

Figure  28-  Followed  trajectories  of  all  vehicles  and  the  reference  trajector>'. 
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Figure  29-  Communication  periods  of  all  vehicles  versus  time. 

Figure  30-  Inputs  applied  to  the  leader  (hovercraft  1 ) 

Figure  31-  Inputs  applied  to  hovercraft  2 

Figure  32-  Change  of  scheduling  parameters  over  time.  (Online  estimation  of  scheduling 

parameters) 


8.3.3  Trajectory  Tracking  of  Miniature  Helicopter  Using  RHC 

The  miniature  helicopter  shown  in  Figure  24  is  used  in  this  test.  The  vision  array  is  used 
to  obtain  necessary  information.  The  experimental  setup  in  this  test  is  presented 
schematically  in  Figure  33. 

Figure  33-  experimental  setup  for  miniature  helicopter 

The  model  used  in  the  controller  is  as  follows: 

(Nominal  model  for  miniature  helicopter) 

The  parameters  of  this  helicopter  were  identified  using  a  similar  approach  explained  in 
[3]  and  are  as  follows: 

(Parameters  of  miniature  helicopter) 

As  mentioned  before,  the  RHC  approach  is  used  and  RllCOOL  [3]  version  4.0  is  used  for 
real-time  implementation.  The  parameters  of  controller  are  as  follows: 

(Controller  parameters) 

The  following  figure,  presents  the  result. 

Figure  34-  Trajectory  of  the  miniature  helicopter  and  the  reference  trajectory 
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8.4  Conclusions 

In  this  report,  a  systematic  approach  is  developed  for  dynamic  scheduling  of  computation 
and  communication  to  determine  the  execution  horizon  and  communication  rate  for 
multiple  uncertain  RHC  systems  on  distributed  processors.  It  was  shown  that  using  the 
scheduling  methods  combined  with  analytical  bounds  on  the  prediction  error,  the  problem 
of  scheduling  multiple  uncertain  plants  can  be  cast  into  an  appropriate  constrained 
optimization  problem.  The  constraints  guarantee  that  the  processes  will  be  schcdulablc, 
both  computation  and  communication,  and  the  optimization  provides  performance 
robustness  to  uncertainty.  The  proposed  method  was  first  applied  via  C++  simulations  to 
a  set  of  hovercrafts  in  formation  (leader-follower)  on  distributed  digital  processors  and  it 
demonstrated  the  validity  of  the  approach.  Performance  and  applicability  of  the  method 
was  also  demonstrated  by  applying  it  experimentally,  to  some  sets  of  under  actuated 
miniature  hovercrafts  in  a  hard  real  time  environment  for  a  leader-follower  example. 
Both  of  the  simulation  and  experiment  were  done  by  developing  an  RllC  Object  Oriented 
Library  (RHCOOL)  in  C++.  This  report  was  explaining  Task  #12,  Task  #13,  and  Task 
#14  of  the  first  proposal. 
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8.6  Appendix  H:  Explanation  of  Attachments 

In  this  chapter,  the  experimental  results  and  the  codes  written  for  the  materials  of  this 
report,  which  are  included  in  the  attached  CD,  is  described.  The  data  is  classified  based 
on  the  different  chapters  of  this  report. 


Chapter  3 

All  of  the  data  used  for  chapter  3  of  the  report  are  included  in  this  folder.  It  includes 
different  subfolders  based  on  the  report  and  the  contents  of  each  folder  are  described 
bellow. 

Section  3.2 

All  of  the  supporting  data  for  section  3.2  of  the  report  is  included  in  this  folder.  It  has  the 
following  subfolders: 


Chapter  6 

In  this  folder,  some  of  the  references  are  included  for  more  convenience  of  the  reader. 
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